//------------------------- Own definitions
#include "globals.h"
#include "main.h"
#include <string.h>
#include <avr/io.h>
#include <avr/wdt.h>
#include <avr/pgmspace.h>
#include <avr/eeprom.h>
#include <util/delay.h>
#include <util/crc16.h>
#include "pid.h"
#include "adc.h"
#include "Timers.h"
#include "Convert.h"
#include "bikirom-ints.h"
#include "logging_sdcard.h"
#include "layer_comm.h"
#include "layer_consult.h"
#include "layer_uart.h"

#if !( defined (__AVR_ATmega1280__) || defined (__AVR_ATmega128__))
#error "No Target device specified!"
#endif

//------
// Macros
#define FrameResponse(x) comm_response(x)

//----------------------------------------------------------------------------
// CUSTOM DEFINITIONS
#define MAIN_WAIT				10
#define MAIN_FRAMES_PERSEC		40
#define ERASE_TIMEOUT			2000
#define MAIN_FRAMES_WAIT		((1000 / MAIN_FRAMES_PERSEC) / MAIN_WAIT)
#define MAIN_CONSULT_TIMEOUT	1000
#define MAIN_CONSULT_COUNTER	(MAIN_CONSULT_TIMEOUT / MAIN_WAIT)
#define FRAME_TIMEOUT			100
#define MAIN_FRAME_NONE			0
#define MAIN_FRAME_BYPASS		1
#define MAIN_FRAME_PASS			2
#define SECURITY_DELAY			4000

//Timer hooks, Array holds function pointer and the countdown and count values
#define TIMERCOUNT			10						// Maximum number of function functions to add
#define TIMERSCHEDULEMS		10
#define TIMERSCHEDULEHZ		(1000/TIMERSCHEDULEMS)
#define CPUWATCH			62

#define THREADS_STOP		m_BikPause = 0x01
#define THREADS_GO			m_BikPause = 0x00
//#define MMC_TEST

//----------------------------------------------------------------------------
// CUSTOM TYPEDEFS
typedef struct _typFlashCopy
{
	uint32_t	nFrom;
	uint32_t	nTo;
	uint32_t	nLength;
} _typFlashCopy;

typedef struct _typAUnit
{
	union {
		float value;
		union {
			unsigned char varValue; 
			unsigned char blank[3];
		};
	} Base;
	union {
		float value;
		union {
			unsigned char varValue; 
			unsigned char blank[3];
		};
	} Component;
	uint8_t	typBase:1,
			typComp:1,
			Operator:6;
} _typAUnit;

typedef struct _typPID
{
	uint8_t	varValue;
	uint8_t Positive:1,Blank:7;
	double iMax;
	double iMin;
	double iGain;
	double pGain;
	double dGain;
	double Position;
	double tGain;
	double MinTime;
	double iValue;
} _typPID;

typedef struct _typEmuTest
{
	unsigned short	REF;
	unsigned short	SYNC;
	unsigned char	Cylinders;
	unsigned char	Mode;
	float	Angles;
	float	RPM;
	float	TEMP;
	float	AFM;
	float	TPS;
} _typEmuTest;

typedef enum _eTurboTimerState {
	eTTWait,
	eTTPreOn,
	eTTOn,
	eTTPreTriggers,
	eTTTriggers,
	eTTPreOff,
	eTTOff
} _eTurboTimerState;

typedef enum _eDataLogState {
	eUnInit,
	eRunning,
	ePaused,
	eNoInit
} _eDataLogState;

typedef struct _typTurboTimer {
	unsigned char			nAuxChannel;
	enum _eTurboTimerState	eLogicState;
	unsigned int			nTimeMin;
	unsigned int			nTimeMax;
	unsigned int			nTimeStart;
} _typTurboTimer;

//----------------------------------------------------------------------------
// STATIC VARIABLES
//static unsigned char	c_Frame_Timer = 0;
static unsigned char			m_Frame_Stream = 0;
static unsigned char			m_Frame_Log = eUnInit;
static uint16_t					m_ADDRESSCOUNTER = 0;
#ifdef EMU_TEST
static struct _typEmuTest		mEmuTest;
#endif
static struct _typTurboTimer	*ptypTurboTimer = NULL;

static uint8_t					m_BikMain = 0x00;
static uint8_t					m_BikPause = 0x00;
static struct _typTimers		m_BikTimers[TIMERCOUNT];

//static struct _typGlobalData	mGlobalData;
//extern static struct _typGlobalData mGlobalData;
static struct _typOutputMem		mOutputMem[OUTPUT_CNT];
unsigned short					TicksPerSec;
struct _typGlobalData			mGlobalData;

#ifdef SIG_WATCHDOG_TIMEOUT
static unsigned char			wdt_watchdog = 0;
#endif


#if HWVER == 31

const unsigned char byteInvert[] PROGMEM = {
0, 128, 64, 192, 32, 160, 96, 224, 16, 144, 80, 208,
48, 176, 112, 240, 8, 136, 72, 200, 40, 168, 104, 232, 
24, 152, 88, 216, 56, 184, 120, 248, 4, 132, 68, 196, 
36, 164, 100, 228, 20, 148, 84, 212, 52, 180, 116, 244, 
12, 140, 76, 204, 44, 172, 108, 236, 28, 156, 92, 220, 60, 
188, 124, 252, 2, 130, 66, 194, 34, 162, 98, 226, 18, 146, 
82, 210, 50, 178, 114, 242, 10, 138, 74, 202, 42, 170, 106, 
234, 26, 154, 90, 218, 58, 186, 122, 250, 6, 134, 70, 198, 38,
166, 102, 230, 22, 150, 86, 214, 54, 182, 118, 246, 14, 142, 
78, 206, 46, 174, 110, 238, 30, 158, 94, 222, 62, 190, 126, 254,
1, 129, 65, 193, 33, 161, 97, 225, 17, 145, 81, 209, 49, 177, 
113, 241, 9, 137, 73, 201, 41, 169, 105, 233, 25, 153, 89, 217, 57,
185, 121, 249, 5, 133, 69, 197, 37, 165, 101, 229, 21, 149, 85, 
213, 53, 181, 117, 245, 13, 141, 77, 205, 45, 173, 109, 237, 29, 
157, 93, 221, 61, 189, 125, 253, 3, 131, 67, 195, 35, 163, 99, 
227, 19, 147, 83, 211, 51, 179, 115, 243, 11, 139, 75, 203, 43, 
171, 107, 235, 27, 155, 91, 219, 59, 187, 123, 251, 7, 135, 71, 
199, 39, 167, 103, 231, 23, 151, 87, 215, 55, 183, 119, 247, 15,
143, 79, 207, 47, 175, 111, 239, 31, 159, 95, 223, 63, 191, 127, 255};

#endif

//----------------------------------------------------------------------------
// LOCAL FUNCTION DEFINITIONS
void					OutputTurboTimer		(unsigned char state);
void					tmrOutputTurboTimer		(void* typTimer);
uint8_t					diagWritePattern		(void);
void 					trace_clear				(void);
void					ADlookupCheck			(void);
inline static uint8_t	TriggerCheck			(uint16_t Address);
inline static uint8_t	logicalOperation		(uint8_t* Operation, uint8_t* ValOne, uint8_t* ValTwo);
void tmrTest2(void* typTimer);
void tmrGetAdc(void* typTimer);

//
#if HWVER==31
static unsigned short	m_TraceAddress = 0; //16-bit address
volatile unsigned char	m_TraceLastData=0;
static unsigned short	m_TraceLastAdr=0;

void trace_clear(void)
{
	uint8_t i;

	for (i=0;i<16;i++)
	{
		mGlobalData.mTraceData.Data[i] = 0;
	}
}

void trace_check(void)
{
	unsigned char trace;

	if (m_TraceAddress != m_TraceLastAdr)
	{
		trace_clear();
		m_TraceLastAdr = m_TraceAddress;
	}

	ccbi(PORT_TRCOE);
	outb(DDR_LADR,0x1);
	outb(DDR_MADR,0xFF);
	outb(PORT_LADR,(m_TraceAddress>>7)&0x01);

	cli();
	PORT_ADR_SET_M(m_TraceAddress>>8);
	trace = pgm_read_byte(&byteInvert[inb(PIN_LADR) & 0xFE]);
	sei();

	if (trace != m_TraceLastData)
	{
		mGlobalData.mTraceData.Data[trace>>3] |= 1 << (trace % 8);
		m_TraceLastData = trace;
	}
}
#endif

void Initialise(void)
{
	uint8_t		i;
	uint32_t	conBitSet;
	
	// Null the outputs
	for (i=0;i<OUTPUT_CNT;i++) {
		memset(&mOutputMem[i],0x00,sizeof(struct _typOutputMem));
	}

	// Null out all function pointers.
	for(i=0;i<TIMERCOUNT;i++)
	{
		m_BikTimers[i].mFncPtr = NULL;
	}

	// Initialise the Consult Uart
	conBitSet = (uint32_t)eeprom_read_byte((uint8_t*)EE_CONSULT_BITSET) << 24;
	conBitSet |=(uint32_t)eeprom_read_byte((uint8_t*)(EE_CONSULT_BITSET+1)) << 16;
	conBitSet |=(uint32_t)eeprom_read_byte((uint8_t*)(EE_CONSULT_BITSET+2)) << 8;
	conBitSet |= (uint32_t)eeprom_read_byte((uint8_t*)(EE_CONSULT_BITSET+3));
	conBitSet ^= (uint32_t)0xFFFFFFFF;
	conBitSet |= (uint32_t)CONSULT_DETAULTS;
	consult_init(setConsultPacket, conBitSet);
	consult_init_comm();

	/*setUartConsultBitSet(((((uint32_t)eeprom_read_byte((void*)EE_CONSULT_BITSET) << 24)
		| ((uint32_t)eeprom_read_byte((void*)(EE_CONSULT_BITSET+1)) << 16)
		| ((uint32_t)eeprom_read_byte((void*)(EE_CONSULT_BITSET+2)) << 8)
		| eeprom_read_byte((void*)(EE_CONSULT_BITSET+3))) ^ 0xFFFFFFFF) | CONSULT_DETAULTS);*/

	ADlookupCheck();
	InitialiseIO();

	AddressReset();
	ADCInit();

	// Enabled WatchDog timer of 60ms
//	wdt_enable(WDTO_2S);

	// Register the Timers
#ifdef DEBUG
//	BikTimerInit(&tmrTest			,0	,4000	,0	,FALSE	,1);
//	BikTimerInit(&tmrTest2			,0	,100	,0	,TRUE	,1);
#endif
	BikTimerInit(&tmrGetInputs		,0	,10		,0	,TRUE	,3);

	BikTimerInit(&tmrGetAdc			,0	,20		,0	,TRUE	,3);
	BikTimerInit(&tmrTriggerSet		,0	,30		,0	,TRUE	,4);
#ifndef EMUTEST
	BikTimerInit(&tmrOutputsSet		,0	,10		,0	,TRUE	,4);

	//BikTimerInit(&tmrOutputsSet		,1	,50		,10	,TRUE	,4);
	//BikTimerInit(&tmrOutputsSet		,2	,50		,20	,TRUE	,4);
	//BikTimerInit(&tmrOutputsSet		,3	,50		,30	,TRUE	,4);
#if HWVER == 2
	//BikTimerInit(&tmrOutputsSet		,4	,100	,40	,TRUE	,4);
#endif

#else
	//BikTimerInit(&tmrOutputsSet		,1	,50		,10	,TRUE	,4);
	//BikTimerInit(&tmrOutputsSet		,3	,50		,30	,TRUE	,4);
#endif
	BikTimerInit(&tmrADLookup		,0	,90		,10, TRUE	,5);
	BikTimerInit(&tmrFrameUpload	,0	,40		,0	,TRUE	,5);
	BikTimerInit(&tmrConsultInit	,0	,500	,0	,TRUE	,5);
	BikTimerInit(&tmrCpuStats		,0  ,1000	,0	,TRUE	,5);

	//	BikTimerInit(&tmrFrameRecieve	,0	,1000	,0	,TRUE	,5);
	// Initialise the test PID calculation! 100ms.. 10hz!
#ifdef PIDTEST
#warning "PID Test Enabled"
	//
#endif

	// Timer Initialisation
	// Initialise this timer exactly
	timer_initialise_mode_exact(0,TIMER_CLK_DIV1024,0xFF,TIMER_CTC);
	TICKS_PER_SEC = (F_CPU / 1024);
	
#if HWVER == 31
	// Initialise the consult clocking
	ssbi(DDR_CCLK);
	timer_initialise_mode(5, 153600, TIMER_PWM);
	timer_5_pwm_A_set(0x01);
	timer_5_pwm_A_set_value(0.5);
#endif
	timer_initialise_mode(THREAD_TIMER_NO, TIMERSCHEDULEHZ, TIMER_CTC);
	timer_attach(THREAD_TIMER, &BikTimerMaster);

	// Initialise the global varialbes
	mGlobalData.mCPUInfo.nTicksMS = 0;
	mGlobalData.mTraceData.Address = 0x0;
	mGlobalData.Protocol = PROTOCOL;

#ifdef EMUTEST
	EmulatorRPMInitialise();
	BikTimerInit(&tmrEmulator		,0	,50		,0	,TRUE	,3);
#endif
	sei();

}
#ifdef DEBUG
#warning "DEBUG DEFINED!!!"
// Test function
void tmrTest(void* typTimer)
{
//	_typTimers*			Timer = (_typTimers*)typTimer;	
	uint8_t		cnt = 10;

	THREADS_STOP;

//	if (log_read_seek(1,0))
	{
		if (log_read_report_usage())
		{
			log_read_report_info(1);
			log_read_report_info(2);
			log_read_report_info(3);
			log_read_report_info(4);
			log_read_report_info(5);
			log_read_report_info(6);
		}
		else
		{
			FrameResponse(0xFE);	
		}
		
		/*while (log_read_next() && --cnt)
		{
			Delay(10);
		};*/

		/*if (log_read_seek(1,0) && log_read_next())
		{
			cnt = 10;

			while (log_read_compressed_next())
			{
				Delay(10);
			};
		}*/
	}
//	else
//	{
//		FrameResponse(0xEF);	
//	}

	THREADS_GO;
}
void tmrTest2(void* typTimer)
{
//	_typTimers*			Timer = (_typTimers*)typTimer;
}
#endif
// delay for a minimum of <us> microseconds 
// the time resolution is dependent on the time the loop takes 
// e.g. with 4Mhz and 5 cycles per loop, the resolution is 1.25 us 
void delay_us(unsigned short time_us) 
{
	unsigned short delay_loops;
	register unsigned short i;

	delay_loops = (time_us+3)/5*CYCLES_PER_US; // +3 for rounding up (dirty) 

	// one loop takes 5 cpu cycles 
	for (i=0; i < delay_loops; i++) {};
}
/* FUNCTION
/	Bik Timer Master Overflow function
/  DESCRIPTION
/	This function is attached to Timer 2 on initialisation.
/	It loops thru the array of software timers and works out
/	if they need to be fired or not.

*/
void BikTimerMaster(void)
{
	unsigned char i=0;

	mGlobalData.mCPUInfo.nTicksMS += 1;
	wdt_reset();

	if (m_BikPause)
		return;

	outb(TICKS_CNT,0);

	// Only go forward if you can, no nested interrupts!
	if (m_BikMain)
		return;
	else
		m_BikMain = TRUE;

	sei();
	
	for(i=0;i<TIMERCOUNT;i++)
	{
		if (m_BikTimers[i].mFncPtr != NULL)
		{
			// Increment the overflow counter
			m_BikTimers[i].mOverflowCnt++;
			// Execute the function is the delay has been met
			if (m_BikTimers[i].mOverflowCnt >= m_BikTimers[i].mOverflowReq)
			{
				// Prevent the thread from being executed again if it is already being run
				if (m_BikTimers[i].mRunning == 0)
				{
					m_BikMain = i + 1;
					// Lock the thread
					m_BikTimers[i].mRunning = 1;
					m_BikTimers[i].mFncPtr(&m_BikTimers[i]);
					// Reset the Watch Dog Timer
					wdt_reset();
					// Unlock the thread
					m_BikTimers[i].mRunning = 0;

					if (!m_BikTimers[i].mBRecuring)
					{	// Once off timer, Null the Pointer so that it doesn't execute again
						m_BikTimers[i].mFncPtr = NULL;
					}
					m_BikTimers[i].mOverflowCnt -= m_BikTimers[i].mOverflowReq;
				}
			}
		}
	}

	mGlobalData.mCPUInfo.nTicks += inb(TICKS_CNT);
	m_BikMain = FALSE;
}

/* FUNCTION
/	Software Timer Init
/  DESCRIPTION
/	Initialises a Bik Timer
/  PARAMATERS
/	BikTmrFunc	:=	Pointer to function with proto fnc(uint16_t)
/	Paramater	:=	Paramater to pass to above function
/	Timeout		:=	Timeout value in ms for the timer
/	Recuring	:=	Specifies if this is a once off timer
/  RETURN
/	False if buffer is full and cannot create a software timer
*/
uint8_t BikTimerInit(void (*BikTmrfunc)(void* param), int16_t Paramater, uint32_t Timeout, uint32_t TimeoutOffset, uint8_t Recuring, uint8_t Priority)
{
	uint8_t i;
	for( i=0;i<TIMERCOUNT;i++)
	{
		if (m_BikTimers[i].mFncPtr == NULL)
		{
			m_BikTimers[i].mFncPtr = BikTmrfunc;
			m_BikTimers[i].mFncParam = Paramater;
			m_BikTimers[i].mOverflowCnt = TimeoutOffset / TIMERSCHEDULEMS;
			m_BikTimers[i].mOverflowReq = Timeout / TIMERSCHEDULEMS;
			m_BikTimers[i].mBRecuring = Recuring;
			m_BikTimers[i].mPriority = Priority;

			return TRUE;
		}
	}

	// Should never get to here, but if we do... Should most probably send up an error message here to
	// Bikisoft, saying something like, timer buffer overflow or something..
	return FALSE;
}

uint8_t	BikTimerDestroy(void (*BikTmrfunc)(void* param))
{
	uint8_t i;
	for( i=0;i<TIMERCOUNT;i++)
	{
		if (m_BikTimers[i].mFncPtr == BikTmrfunc)
		{
			m_BikTimers[i].mFncPtr = NULL;

			return TRUE;
		}
	}

	// Should never get to here, but if we do... Should most probably send up an error message here to
	// Bikisoft, saying something like, timer buffer overflow or something..
	return FALSE;
}

void InitialiseIO(void)
{
	float	nPwmHz;

	// Initialise the ADC Port first
	outb(DDRF, 0x00);
	outb(PORTF, 0x00);

#if HWVER != 31
	// Set all as output and Hi for Command Patterns
	ssbi(PORT_CMDOE);
	ssbi(PORT_CMDEV);
	ssbi(PORT_CMDOD);
	ssbi(DDR_CMDOE);
	ssbi(DDR_CMDEV);
	ssbi(DDR_CMDOD);

	// Set all as output and Hi for Address Counter
	ssbi(PORT_AACOE);
	ssbi(PORT_AACUP);
	ssbi(PORT_AACCLR);
	ssbi(DDR_AACOE);
	ssbi(DDR_AACUP);
	ssbi(DDR_AACCLR);
#endif

	// Set all the initial address registers etc
	ccbi(PORT_BBA15);
	ccbi(PORT_BBA16);
	ssbi(DDR_BBA15);
	ssbi(DDR_BBA16);

#if HWVER == 2
	ccbi(PORT_ABOE);
	ccbi(PORT_OED);

	ccbi(DDR_ABOE);
	ccbi(DDR_OED);

	// Set OE and CE as inputs
	ccbi(DDR_AOE);
	ccbi(DDR_ACE);
#else
	ccbi(PORT_BNKSEL);
	ssbi(DDR_BNKSEL);

	// Set OE and CE as outputs
	ssbi(DDR_AOE);
	ssbi(DDR_ACE);

	// Set all to initial states
	ssbi(PORT_AOE);
	ssbi(PORT_ACE);

	// Set the upper address ports
	ccbi(PORT_BBA17);
	ccbi(PORT_BBA18);
	ssbi(DDR_BBA17);
	ssbi(DDR_BBA18);

	ccbi(DDR_ARB);
	ccbi(PORT_ARB);
#endif

	// Hold Reset High always
	ssbi(PORT_ARP);
	ssbi(DDR_ARP);
	
	// Hold WE High always
	ssbi(PORT_AWE);
	ssbi(DDR_AWE);
	
	// Set the flash thingo's as inputs
	DDR_BU1 = 0x00;
	DDR_BU2 = 0x00;

	// Auxiliary Channels
	nPwmHz = getFloatS(EE_BK1 + 0xFC);
	if (nPwmHz == 0) {
		ccbi(PORT_AUX1);
	} else {
		AUX_OUT_12_SET_PWM(nPwmHz);
	}

	nPwmHz = getFloatS(EE_BK3 + 0xFC);
	if (nPwmHz == 0) {
		ccbi(PORT_AUX3);
	} else {
		AUX_OUT_34_SET_PWM(nPwmHz);
	}

	ssbi(DDR_AUX1);
	ssbi(DDR_AUX2);
	ssbi(DDR_AUX3);
	ssbi(DDR_AUX4);
	ccbi(PORT_AUX1);
	ccbi(PORT_AUX2);
	ccbi(PORT_AUX3);
	ccbi(PORT_AUX4);

#if HWVER==31
	ccbi(MMC_DDR);	/* Set SD Card Chip Detect as input */
	ssbi(DDR_TRCOE);
	ssbi(PORT_TRCOE);
#endif
}

//----------------------------------------- Rx Command Parsing

/*void FrameResponse(unsigned char Response) // Standard Frame Responses
{
	unsigned int crc = 0;
	crc = _crc16_update(crc, ID_LAST_COMMAND_STATUS);
	crc = _crc16_update(crc, Response);

	unsigned char Frame[12] = {0xAA,0xA5,0x1A,0x2A,0x00,0x02,ID_LAST_COMMAND_STATUS,Response,crc >> 8,crc,0x55,0x5A};

	uartSendBuffer(0,&Frame[0],12);
	sei();

	uart1Check();
}*/

void FrameUpload(void)
{
	uint8_t	chHdr[2] = {ID_CONSULT_INTEGRATED};
	comm_new	(eMsgPc,eMsgAtmel,sizeof(_typGlobalData)+1);
	comm_append	(chHdr,1);
	comm_append	((uint8_t*)&mGlobalData, sizeof( _typGlobalData ));
	comm_send	();

	// Clear the last trace buffer
	trace_clear();
}

void FrameTestOutputs(void)
{
	// Destroy the timer
	BikTimerDestroy(&tmrOutputsSet);

#ifdef AUX_OUT_12_SET_PWM
	AUX_OUT_12_SET_PWM(100);
	AUX_OUT_1_SET(1);
	AUX_OUT_1_SET_VALUE(0.50);
#if HWVER == 31
	AUX_OUT_2_SET(1);
	AUX_OUT_2_SET_VALUE(0.50);
#endif
#endif

#ifdef AUX_OUT_34_SET_PWM
	AUX_OUT_34_SET_PWM(100);
	AUX_OUT_3_SET(1);
	AUX_OUT_3_SET_VALUE(0.50);
#if HWVER == 31
	AUX_OUT_4_SET(1);
	AUX_OUT_4_SET_VALUE(0.50);
#endif
#endif

	FrameResponse(CMD_PASSED);
}

void FrameGetADC(void)
{
	_typAdcData	typAdcData;

	typAdcData.nId = ID_GENERAL_SPECIFIC;

	typAdcData.nAdc[0] = mGlobalData.AdInputs[0].Raw;
	typAdcData.nAdc[1] = mGlobalData.AdInputs[1].Raw;
	typAdcData.nAdc[2] = mGlobalData.AdInputs[2].Raw;
#if HWVER == 31
	typAdcData.nAdc[3] = mGlobalData.SwAdcInputs.value[0];
	typAdcData.nAdc[4] = mGlobalData.SwAdcInputs.value[1];
	typAdcData.nAdc[5] = mGlobalData.SwAdcInputs.value[2];
	typAdcData.nAdc[6] = mGlobalData.SwAdcInputs.value[3];
	typAdcData.nAdc[7] = mGlobalData.SwAdcInputs.value[4];
#else
	typAdcData.nAdc[3] = 0;
	typAdcData.nAdc[4] = 0;
	typAdcData.nAdc[5] = 0;
	typAdcData.nAdc[6] = 0;
	typAdcData.nAdc[7] = 0;
#endif

	comm_new	(eMsgPc,eMsgAtmel,sizeof(_typAdcData));
	comm_append	((unsigned char*)&typAdcData, sizeof( _typAdcData ));
	comm_send	();
}

void FrameGetInfo(void)
{
	uint8_t	chHdr[] = {ID_GENERAL_SPECIFIC,
						eeprom_read_byte((void*)EE_BOARD_SERIAL_NO),
						eeprom_read_byte((void*)EE_BOARD_SERIAL_NO+1),
						eeprom_read_byte((void*)EE_BOARD_SERIAL_NO+2),
						eeprom_read_byte((void*)EE_USERBYTE),
						eeprom_read_byte((void*)EE_SECURITYBYTE),
						SPEED,
						VERSION};
	uint8_t	chFtr[] = {consult_get_status(),
						ADC_MAX & 0xFF,
						ADC_MAX >> 8};

	comm_new	(eMsgPc,eMsgAtmel,sizeof(mGlobalData.mCPUInfo.mCPUStats)+sizeof(chHdr)+sizeof(chFtr));
	comm_append	(chHdr,sizeof(chHdr));
	comm_append	((uint8_t*)&mGlobalData.mCPUInfo.mCPUStats,sizeof(mGlobalData.mCPUInfo.mCPUStats));
	comm_append	(chFtr,sizeof(chFtr));
	comm_send	();
}

void comm_usb_handler(uint8_t* buffer, uint16_t nLength)
{
	unsigned char *p = &buffer[6];
	uint8_t				Mode;

	if (comm_to(buffer) == eMsgAtmel)
	{
		switch (*p++)
		{

		case ID_FLASH_COPY:
			{
				_typFlashCopy	*ptypData = (_typFlashCopy*)p;
				if ((ptypData->nFrom+ptypData->nLength <= FLASH_END)
				  &&(ptypData->nTo+ptypData->nLength <= FLASH_END)
				  &&(ptypData->nLength >= 0x02))
				{
					flashCopy(&ptypData->nFrom, &ptypData->nTo, &ptypData->nLength);
				}
				else
				{
					FrameResponse(CMD_FAILED);
				}
			}
			break;
		case ID_FLASH_DATA:
			{
				uint32_t	Address;
				uint16_t	Length;
				Mode = *p++;
				Address = (uint32_t)*p++ << 16;
				Address |= (uint32_t)*p++ << 8;
				Address |= *p++;
				Length = (uint16_t)*p++ << 8;
				Length |= *p++;
				if (((Address + Length) > (FLASH_END+1)))
					FrameResponse(CMD_FAILED);
				else if (Mode == WRITE)
					if (SecurityCheck(SEC_LV1))
						if (Length <= (nLength-6))
							flashWrite(&Address, &Length, p);
						else
							FrameResponse(CMD_FAILED);
					else
						FrameResponse(SEC_FUNCTION_LOCKED);
				else if (Mode == READ)
					if (SecurityCheck(SEC_LV4))
						flashRead(&Address, &Length);
					else
						FrameResponse(SEC_FUNCTION_LOCKED);
				else
					FrameResponse(CMD_FAILED);
			}
			break;
		case ID_EEPROM_DATA:
			{
				uint32_t	Address;
				uint16_t	Length;
				Mode = *p++;
				Address = (uint32_t)*p++ << 16;
				Address |= (uint32_t)*p++ << 8;
				Address |= *p++;
				Length = (uint16_t)*p++ << 8;
				Length |= *p++;
				if ((Address + Length) > (EE_END+1))
					FrameResponse(CMD_FAILED);
				else if(Mode == WRITE)
		#ifndef DEBUG_MODE
					if (SecurityCheck(SEC_LV2))
						if (Length <= (nLength-6))
		#endif
							eepromWrite((uint16_t)Address, &Length, p);
		#ifndef DEBUG_MODE
						else
							FrameResponse(CMD_FAILED);
					else
						FrameResponse(SEC_FUNCTION_LOCKED);
		#endif
				else if(Mode == READ)
		#ifndef DEBUG_MODE
					if (SecurityCheck(SEC_LV4))
		#endif
						eepromRead((uint16_t)Address, &Length);
		#ifndef DEBUG_MODE
					else
						FrameResponse(SEC_FUNCTION_LOCKED);
		#endif
				else
					FrameResponse(CMD_FAILED);
			}
			break;
		case ID_RUN_COMMAND:
		
			Mode = *p++;
			switch (Mode)
			{
#if HWVER == 31
			case eCmdRunGetLogData:
				{
					_typLogSeek	*ptypLogSeek = (_typLogSeek*)p;
					if (log_read_seek(ptypLogSeek->nLogNo, ptypLogSeek->nSeqNo))
					{
						log_read_next();
					}
					else
					{
						FrameResponse(CMD_FAILED);
					}
				}
				break;

			case eCmdRunGetLogCompData:
				{
					_typLogSeek	*ptypLogSeek = (_typLogSeek*)p;
					if (log_read_seek(ptypLogSeek->nLogNo, ptypLogSeek->nSeqNo))
					{
						log_read_compressed_next();
					}
					else
					{
						FrameResponse(CMD_FAILED);
					}
				}
				break;

			case eCmdRunGetLogReport:
				log_read_report_usage();
				break;

			case eCmdRunGetLogInfo:
				log_read_report_info(*(uint32_t*)p);
				break;

			case eCmdRunSetLogSeek:
				{
					_typLogSeek	*ptypLogSeek = (_typLogSeek*)p;
					if (log_read_seek(ptypLogSeek->nLogNo, ptypLogSeek->nSeqNo))
					{
						FrameResponse(CMD_OK);
					}
					else
					{
						FrameResponse(CMD_FAILED);
					}
				}

				break;
			case eCmdRunDataLogging:
				{
					if (*p++)	// enable datalogging
					{
						if (m_Frame_Log == ePaused)
						{
							m_Frame_Log = eRunning;
						}
						else if (m_Frame_Log == eNoInit)
						{
							m_Frame_Log = eUnInit;
						}
					}
					else		// Disable datalogging
					{
						if (m_Frame_Log == eRunning)
						{
							m_Frame_Log = ePaused;
						}
						else if (m_Frame_Log == eUnInit)
						{
							m_Frame_Log = eNoInit;
						}
					}
					FrameResponse(CMD_OK);
				}
				break;

			case eCmdRunDataLogErase:
				{
					uint32_t	dts = *(uint32_t*)p;

					// Set the eeprom data
					eeprom_write_block(&dts,(void*)EE_SD_DTS, sizeof(dts));
					
					// Unititialise the data logging
					m_Frame_Log = eUnInit;

					// Re-initialise the sd card
					if (!bit_is_clr(MMC_CD) && log_init(dts))
					{
						m_Frame_Log = eRunning;

						// Send back the response
						FrameResponse(CMD_OK);
					}
					else
						FrameResponse(CMD_FAILED);
				}

				break;
			case eCmdRunSetTraceAdr:
				mGlobalData.mTraceData.Address = *(uint16_t*)p;
				m_TraceAddress = mGlobalData.mTraceData.Address;
				FrameResponse(CMD_OK);
				break;
#endif
			case RUN_ERASEALL:
			case RUN_ERASEALL_BANK1:
			case RUN_ERASEALL_BANK2:
				if (SecurityCheck(SEC_LV1))
				{
					if (flashEraseAll(Mode & 0x01))
					{
						FrameResponse(CMD_OK);
					}
					else
					{
						FrameResponse(CMD_FAILED);
					}
				}
				else
				{
					FrameResponse(SEC_FUNCTION_LOCKED);
				}
				break;
			case RUN_ERASESECTOR:
				{
					uint32_t	Address;

					Address = (uint32_t)*p++ << 16;
					Address |= (uint32_t)*p++ << 8;
					Address |= *p++;
					if (SecurityCheck(SEC_LV1))
					{
						switch (flashErase(&Address))
						{
						case FLASH_OK:
							FrameResponse(CMD_OK);
							break;
						case FLASH_FAIL:
							FrameResponse(FLASH_WRITE_FAILED);
							break;
						case FLASH_TIMEOUT:
							FrameResponse(FLASH_WRITE_TIMEOUT);
							break;
						}
					}
					else
					{
						FrameResponse(SEC_FUNCTION_LOCKED);
					}

				}
				break;
			case RUN_TEST_OUTPUTS:
				FrameTestOutputs();
				break;
			case RUN_GET_BOARD_ADC:
				FrameGetADC();
				break;
			case RUN_GET_INFO:
				FrameGetInfo();
				break;
			case RUN_WRITE_PATTERN:
				if (diagWritePattern())
					FrameResponse(CMD_PASSED);
				else
					FrameResponse(CMD_NOTPASSED);
				break;
			case RUN_DIAG_RXTX:
				if (diagRxTx())
					FrameResponse(CMD_PASSED);
				else
					FrameResponse(CMD_NOTPASSED);
				break;
			case RUN_DIAG_RXCLK:
				if (diagRxClk())
					FrameResponse(CMD_PASSED);
				else
					FrameResponse(CMD_NOTPASSED);
				break;
			case RUN_DIAG_FLASH:
			case RUN_DIAG_FLASH2:
				if (diagFlashMem(Mode == RUN_DIAG_FLASH2 ? 0x01 : 0x00))
					FrameResponse(CMD_PASSED);
				else
					FrameResponse(CMD_NOTPASSED);
				break;
			default:
				if ((Mode >= RUN_SEC_UNLOCKCODE ) && (Mode <= RUN_SEC_SETSECLEVEL) && (nLength >= 18))
					SecuritySet(Mode, p);
				else
					FrameResponse(CMD_FAILED);
			}
			break;
		case ID_CONSULT_INTEGRATED:
			switch (*p++)
			{
			case CMD_GO:
				m_Frame_Stream = MAIN_FRAME_PASS;
				FrameResponse(CMD_OK);
				break;
			case CMD_STOP:
				m_Frame_Stream = MAIN_FRAME_NONE;
				FrameResponse(CMD_OK);
				break;
			default:
				FrameResponse(CMD_FAILED);
				break;
			}
			
			break;

		case ID_CONSULT_PASS:
			switch (*p++)
			{
			case CMD_GO:
				m_Frame_Stream = MAIN_FRAME_BYPASS;
				consult_stop_comm();
				FrameResponse(CMD_OK);
				// Re-set the recieve to goto the USB
				break;

			case CMD_STOP:
				m_Frame_Stream = MAIN_FRAME_NONE;
				FrameResponse(CMD_OK);
				// Re-set the recieve back to the consult
				break;

			default:
				FrameResponse(CMD_FAILED);
			}
			break;
		default:
			FrameResponse(CMD_UNKNOWN);
		}
	}
	else
	{
		FrameResponse(eCmdRespUnknownReciever);
	}
}


/* checks the security byte to see if a function is currently allowed
*/
uint8_t SecurityCheck(uint8_t LevelRequired)
{
	uint8_t SecByte = eeprom_read_byte((const void*)EE_USERBYTE) & 0x07;
	if (SecByte < LevelRequired)
		return 0;
	else
		return 1;
}

/* Checks the Security Unlock byte for a functions ok
*/
uint8_t SecurityUnlockCheck(uint8_t FunctionCode)
{
	uint8_t SecByte = eeprom_read_byte((void*)EE_SECURITYBYTE) ^ 0xFF;
	if (FunctionCode <= 0x0F)
		return SecByte & 0x01;
	else if(FunctionCode <= 0x1F)
		return (SecByte & 0x02) >> 1;
	else if(FunctionCode <= 0x2F)
		return (SecByte & 0x04) >> 2;
	else if(FunctionCode <= 0x3F)
		return (SecByte & 0x08) >> 3;
	else if(FunctionCode <= 0x4F)
		return (SecByte & 0x10) >> 4;
	else if(FunctionCode <= 0x5F)
		return (SecByte & 0x20) >> 5;
	else if(FunctionCode <= 0x6F)
		return (SecByte & 0x40) >> 6;
	else if(FunctionCode <= 0x7F)
		return (SecByte & 0x80) >> 7;
	else
		return 0;
}

void SecuritySet(uint8_t Cmd, uint8_t* Buffer)
{
	uint8_t match = 0;
	uint8_t i=0;
	uint8_t n;
	uint8_t SecByte;
	switch (Cmd)
	{
	case RUN_SEC_UNLOCKCODE:
		for (i=0;i<8;i++)
		{
			match = 0;
			for (n=0;n<16;n++)
			{
				if (Buffer[15-n] != eeprom_read_byte((const void*)(EE_SECCODE_0+(n*8)+i)))
					match = 1;
			}
			if (match == 0)
			{
				Delay(SECURITY_DELAY);
				SecByte = eeprom_read_byte((const void*)EE_SECURITYBYTE) ^ 0xFF;
				SecByte ^= (1 << i);
				eeprom_write_byte((void*)EE_SECURITYBYTE, (SecByte ^ 0xFF));
				break;
			}
		}
		if (match == 0)
			FrameResponse(CMD_OK);
		else
		{
			Delay(SECURITY_DELAY);
			FrameResponse(SEC_BAD_CODE);
		}
		break;
	case RUN_SEC_SETPASSWORD:
		if (SecurityCheck(SEC_LV1))
		{
			for(i=0;i<16;i++)
			{
				eeprom_write_byte((void*)EE_USER0+i,Buffer[i]);
				wdt_reset();
			}
			FrameResponse(CMD_OK);
		} else
		{ FrameResponse(SEC_FUNCTION_LOCKED); }
		break;
	case RUN_SEC_SETSECLEVEL:
		match = 0;
		for (n=0;n<16;n++)
		{
			if (Buffer[n] != eeprom_read_byte((const void*)(EE_USER0+n)))
				match = 1;
		}
		if (match == 0)
		{
			Delay(SECURITY_DELAY);
			SecByte = Buffer[16];
			eeprom_write_byte((void*)EE_USERBYTE, SecByte);
			FrameResponse(CMD_OK);
		} else
		{
			Delay(SECURITY_DELAY);
			FrameResponse(SEC_BAD_CODE);
		}
		break;
	default:
		FrameResponse(CMD_FAILED);
	}
}

inline void resetFlash(void)
{
	ccbi(PORT_ARP);
	Delay(1);
	ssbi(PORT_ARP);
	Delay(1);
}
/* Establish the flash in Read/Write mode.
*/
inline void modeReadWrite(void)
{
	// Set ABOE and OED High to disable the board to ecu buses
#if HWVER == 2
	ssbi(PORT_ABOE);
	ssbi(PORT_OED);
	ssbi(DDR_ABOE);
	ssbi(DDR_OED);

	ssbi(DDR_AOE);
	ssbi(DDR_AWE);
	ssbi(DDR_ACE);
#endif

	resetFlash();

	outb(DDR_BU1, 0x00);
	outb(DDR_BU2, 0x00);

	ssbi(PORT_ARP);
	ssbi(PORT_AOE);
	ssbi(PORT_AWE);
	// Select the chip!
	ccbi(PORT_ACE);

#if HWVER != 31
	ssbi(PORT_CMDOE);
	ssbi(PORT_AACOE);
#else
	ssbi(PORT_TRCOE);
	ADDRESS_OE;
#endif
}

/* does a ms delay resetting the wd timer
*/
void Delay(uint16_t ms)
{
	uint16_t i;
	for (i=0; i<ms;i++)
	{
		wdt_reset();
		_delay_ms(1);
	}
}

// Set AA15,16 to the appropriate map
// Valid values = 0x00, 0x01, 0x02
void SetMap(uint8_t Map)
{
#if HWVER == 2
	switch (Map)
	{
	case 0x00:
		ccbi(PORT_BBA16);
		ccbi(PORT_BBA15);
		break;
	case 0x01:
		ccbi(PORT_BBA16);
		ssbi(PORT_BBA15);
		break;
	case 0x02:
		ccbi(PORT_BBA15);
		ssbi(PORT_BBA16);
		break;
	default:
		ccbi(PORT_BBA15);
		ccbi(PORT_BBA16);
	}
#else
	if (Map < 0x0F)
	{
		PORT_BBA_SET(Map);
	} else
	{
		PORT_BBA_SET(0);
	}
#endif
}

void modeRun(void)
{
	// Set the flash thingo's as inputs
	outb(DDR_BU1, 0x00);
	outb(DDR_BU2, 0x00);

#if HWVER != 31
	// Set all as output and Hi for Address Counter
	ssbi(PORT_AACOE);
	ssbi(PORT_CMDOE);
#endif

	resetFlash();

	// Set WE High!
	ssbi(PORT_AWE);
	ccbi(PORT_ACE);
	ssbi(PORT_ARP);
#if HWVER == 2
	// Set all the initial address registers etc
	ccbi(PORT_BBA15);
	ccbi(PORT_BBA16);

	ccbi(DDR_AOE);
	ccbi(DDR_ACE);
	ccbi(DDR_ABOE);
	ccbi(DDR_OED);

	ccbi(PORT_ABOE);
	ccbi(PORT_OED);
#else
#if HWVER != 31
	ssbi(PORT_AACOE);
#else
	//ADDRESS_OD;
#endif
	ssbi(PORT_AOE);


#endif

}

/* Assumes the BU1 and BU2 are already set to output
Also assumes that OE is Hi and CE is Lo
*/
void modeSendWriteCmd(void)
{
	outb(DDR_BU1,0xFF);
	outb(DDR_BU2,0xFF);

#if HWVER != 31
	// send 0x5555 & 0xAA
	ssbi(PORT_AACOE);
	ssbi(PORT_CMDEV);
	ccbi(PORT_CMDOD);
	ccbi(PORT_CMDOE);
	outb(PORT_BU2, 0x00);
	outb(PORT_BU1, 0xAA);
	pulseLo(AWE);

	// send 0x2AAA & 0x55
	ccbi(PORT_CMDEV);
	ssbi(PORT_CMDOD);
	outb(PORT_BU1, 0x55);
	pulseLo(AWE);

	// send 0x5555 & 0xA0
	ssbi(PORT_CMDEV);
	ccbi(PORT_CMDOD);
	outb(PORT_BU1, 0xA0);
	pulseLo(AWE);

	// set the CMD to HiZ, set the addr counter to enabled
	ssbi(PORT_CMDOE);
	ccbi(PORT_AACOE);
#else
	PORT_ADR_55;
	outb(PORT_BU2, 0x00);
	outb(PORT_BU1, 0xAA);
	pulseLo(AWE);

	PORT_ADR_AA;
	outb(PORT_BU1, 0x55);
	pulseLo(AWE);

	PORT_ADR_55;
	outb(PORT_BU1, 0xA0);
	pulseLo(AWE);
#endif
}

/* Assumes all outputs setup for flash read
*/
void AddressReset(void)
{
#if HWVER != 31
	pulseLo(AACCLR);
	pulseHi(AACUP);
#endif
	m_ADDRESSCOUNTER = 0;
}

/* Assumes all outputs setup for flash read
*/

void AddressInit(_typAddress* address)
{
#if HWVER == 2
	uint16_t i;
	uint16_t adr = (uint16_t)address->nAddress & (uint16_t)0x7FFF;

	if (adr < m_ADDRESSCOUNTER)
	{
		AddressReset();
	}

	for(i=m_ADDRESSCOUNTER;i<adr;i++)
	{
		pulseHi(AACUP); 
	}

	m_ADDRESSCOUNTER = adr;
	
	if (address->nBytes[1] & 0x80)
	{
		ssbi(PORT_BBA15);
	}
	else
	{
		ccbi(PORT_BBA15);
	}

	if (address->nBytes[2] & 0x01)
	{
		ssbi(PORT_BBA16);
	}
	else
	{
		ccbi(PORT_BBA16);
	}

	
#endif
#if HWVER == 3
	uint32_t adr = *address & (uint32_t)0x0007FFFF; 

	if (adr < m_ADDRESSCOUNTER)
	{
		AddressReset();
	}
	for(uint32_t i=m_ADDRESSCOUNTER;i<adr;i++)
	{
		pulseHi(AACUP); 
	}
	if (address->nBytes[2] & 0x08) //(uint32_t)*address & (uint32_t)0x00080000)
	{
		ssbi(PORT_BNKSEL);
	} else
	{
		ccbi(PORT_BNKSEL);
	}

	m_ADDRESSCOUNTER = adr;
#endif
#if HWVER == 31
	PORT_ADR_SET_H(address->nBytes[2]);
	PORT_ADR_SET_M(address->nBytes[1]);
	PORT_ADR_SET_L(address->nBytes[0]);

	if (address->nBytes[2] & 0x08)
		ssbi(PORT_BNKSEL);
	else
		ccbi(PORT_BNKSEL);
#endif
}

// Must Setup all outputs for flash read
// Assumes no rolover

//  ------------------ TRIAL!!!!
void flashRead(uint32_t* address, uint16_t* length)
{
	uint16_t i;
	THREADS_STOP;
	*length &= 0xFFFE;

	uint8_t		packetHead[6] = {ID_FLASH_DATA
								,(uint8_t)(*address >> 16)
								,(uint8_t)(*address >> 8)
								,(uint8_t)*address
								,(uint8_t)(*length >> 8)
								,(uint8_t)*length};
	uint8_t		data[2];

	// ensure only even numbers of bytes
	
	comm_new(eMsgPc,eMsgAtmel, *length + sizeof(packetHead));
	comm_append(packetHead,sizeof(packetHead));
	
	// Downgrade the address to 16bit
	*address = *address >> 1;

	modeReadWrite();
	ccbi(PORT_ACE);
	AddressInit((_typAddress*)address);

#if HWVER != 31
	ccbi(PORT_AACOE);
#endif
	for(i=12;i<(*length+12);i+=2)
	{
		AddressInit((_typAddress*)address);
		ccbi(PORT_AOE);
		ccbi(PORT_AOE);
		//ccbi(PORT_AACOE);
		data[0] = inb(PIN_BU2);
		data[1] = inb(PIN_BU1);

		ssbi(PORT_AOE);

		// calculate CRC and send bytes
		comm_append(data,2);

		*address += 1;
	}

	comm_send();

	THREADS_GO;

	modeRun();
}

uint8_t flashEraseAll(uint8_t Bank)
{
	cli();
	uint16_t i;
	uint8_t bStatus = 1;
	uint8_t nLast=0;
	uint32_t nThis=0;
	uint32_t nTimeout=(uint32_t)TICKS_PER_SEC * (uint32_t)30;

	THREADS_STOP;

	// Select the correct memory bank before erase
#if HWVER == 3 || HWVER == 31
	if (Bank)
		ssbi(PORT_BNKSEL);
	else
		ccbi(PORT_BNKSEL);
#endif

	modeReadWrite();

	outb(DDR_BU1, 0xFF);
	outb(DDR_BU2, 0xFF);

	// set ACE Lo
	ccbi(PORT_ACE);
	ssbi(DDR_ACE);

	// Set AOE Hi
	ssbi(PORT_AOE);
	ssbi(DDR_AOE);


#if HWVER != 31
	// send 0x5555 & 0xAA
	ssbi(PORT_AACOE);
	ssbi(PORT_CMDEV);
	ccbi(PORT_CMDOD);
	ccbi(PORT_CMDOE);
	outb(PORT_BU1, 0xAA);
	pulseLo(AWE);

	// send 0x2AAA & 0x55
	ccbi(PORT_CMDEV);
	ssbi(PORT_CMDOD);
	outb(PORT_BU1, 0x55);
	pulseLo(AWE);

	// send 0x5555 & 0x80
	ssbi(PORT_CMDEV);
	ccbi(PORT_CMDOD);
	outb(PORT_BU1, 0x80);
	pulseLo(AWE);

	// send 0x5555 & 0xAA
	ssbi(PORT_CMDEV);
	ccbi(PORT_CMDOD);
	outb(PORT_BU1, 0xAA);
	pulseLo(AWE);

	// send 0x2AAA & 0x55
	ccbi(PORT_CMDEV);
	ssbi(PORT_CMDOD);
	outb(PORT_BU1, 0x55);
	pulseLo(AWE);

	// send 0x5555 & 0x10
	//PORTG ^= PG0 | PG3;
	ssbi(PORT_CMDEV);
	ccbi(PORT_CMDOD);
	outb(PORT_BU1, 0x10);
	pulseLo(AWE);

#else

	// send 0x0555 & 0xAA
	PORT_ADR_SET_H(0x00);
	PORT_ADR_SET_M(0x05);
	PORT_ADR_SET_L(0x55);
	outb(PORT_BU2, 0x00);
	outb(PORT_BU1, 0xAA);
	pulseLo(AWE);

	// send 0x02AA & 0x55
	PORT_ADR_SET_M(0x02);
	PORT_ADR_SET_L(0xAA);
	outb(PORT_BU1, 0x55);
	pulseLo(AWE);

	// send 0x0555 & 0x80
	PORT_ADR_SET_M(0x05);
	PORT_ADR_SET_L(0x55);
	outb(PORT_BU1, 0x80);
	pulseLo(AWE);

	// send 0x0555 & 0xAA
	PORT_ADR_SET_M(0x05);
	PORT_ADR_SET_L(0x55);
	outb(PORT_BU1, 0xAA);
	pulseLo(AWE);

	// send 0x02AA & 0x55
	PORT_ADR_SET_M(0x02);
	PORT_ADR_SET_L(0xAA);
	outb(PORT_BU1, 0x55);
	pulseLo(AWE);

	// send 0x0555 & 0x10
	PORT_ADR_SET_M(0x05);
	PORT_ADR_SET_L(0x55);
	outb(PORT_BU1, 0x10);
	pulseLo(AWE);

#endif

	outb(DDR_BU1, 0x00);
	outb(DDR_BU2, 0x00);

	Delay(1);

	i = CPU_TICKS;

	while ( bStatus )
	{
		if (TICKS_CNT > nLast)
			nThis += TICKS_CNT - nLast;
		nLast = TICKS_CNT;
		// Erase Timed Out
		if (nThis > nTimeout )
		{
			bStatus = FLASH_FAIL;
		// Erase is finished
		} else if (!bit_is_clr(PIN_ARB))
		{
			break;
		} else {
			Delay(10);
		}
	}

	resetFlash();

	modeRun();
	THREADS_GO;

	return bStatus;
}

uint8_t flashErase(uint32_t* Address)
{
	cli();
	//uint32_t i;
	uint8_t bStatus = FLASH_OK;
	uint8_t nLast=0;
	uint8_t	nTime=0;
	uint16_t nThis=0;
	uint16_t nTimeout=0;

	THREADS_STOP;
	
	/* Initialise the address */
	*Address = *Address >> 1;
	AddressInit((_typAddress*)Address);
	
	modeReadWrite();

	outb(DDR_BU1, 0xFF);
	outb(DDR_BU2, 0xFF);

	// set ACE Lo
	ccbi(PORT_ACE);
	ssbi(DDR_ACE);

	// Set AOE Hi
	ssbi(PORT_AOE);
	ssbi(DDR_AOE);

#if HWVER != 31
	// send 0x555 & 0xAA
	ssbi(PORT_AACOE);
	ssbi(PORT_CMDEV);
	ccbi(PORT_CMDOD);
	ccbi(PORT_CMDOE);
	outb(PORT_BU2, 0x00);
	outb(PORT_BU1, 0xAA);
	pulseLo(AWE);
	//return 1;

	// send 0x2AA & 0x55
	//PORTG ^= PG0 | PG3;
	ccbi(PORT_CMDEV);
	ssbi(PORT_CMDOD);
	outb(PORT_BU1, 0x55);
	pulseLo(AWE);

	// send 0x555 & 0x80
	//PORTG ^= PG0 | PG3;
	ssbi(PORT_CMDEV);
	ccbi(PORT_CMDOD);
	outb(PORT_BU1, 0x80);
	pulseLo(AWE);

	// send 0x555 & 0xAA
	//PORTG ^= PG0 | PG3;
	ssbi(PORT_CMDEV);
	ccbi(PORT_CMDOD);
	outb(PORT_BU1, 0xAA);
	pulseLo(AWE);

	// send 0x2AA & 0x55
	//PORTG ^= PG0 | PG3;
	ccbi(PORT_CMDEV);
	ssbi(PORT_CMDOD);
	outb(PORT_BU1, 0x55);
	pulseLo(AWE);

	// send 0x555 & 0x30
	//PORTG ^= PG0 | PG3;
	ssbi(PORT_CMDOE);
	//AddressInit(Address);
	ccbi(PORT_AACOE);
	outb(PORT_BU1, 0x30);
	pulseLo(AWE);
#else
	// send 0x555 & 0xAA
	PORT_ADR_SET_H(0x00);
	PORT_ADR_SET_M(0x05);
	PORT_ADR_SET_L(0x55);
	outb(PORT_BU2, 0x00);
	outb(PORT_BU1, 0xAA);
	pulseLo(AWE);

	// send 0x2AA & 0x55
	PORT_ADR_SET_M(0x02);
	PORT_ADR_SET_L(0xAA);
	outb(PORT_BU1, 0x55);
	pulseLo(AWE);

	// send 0x555 & 0x80
	PORT_ADR_SET_M(0x05);
	PORT_ADR_SET_L(0x55);
	outb(PORT_BU1, 0x80);
	pulseLo(AWE);

	// send 0x555 & 0xAA
	PORT_ADR_SET_M(0x05);
	PORT_ADR_SET_L(0x55);
	outb(PORT_BU1, 0xAA);
	pulseLo(AWE);

	// send 0x2AA & 0x55
	PORT_ADR_SET_M(0x02);
	PORT_ADR_SET_L(0xAA);
	outb(PORT_BU1, 0x55);
	pulseLo(AWE);

	// send ADRX & 0x30
	AddressInit((_typAddress*)Address);
	outb(PORT_BU1, 0x30);
	pulseLo(AWE);

#endif

	outb(DDR_BU1, 0x00);
	outb(DDR_BU2, 0x00);

	ccbi(PORT_AOE);
	nLast = PIN_BU1;
	ssbi(PORT_AOE);
	nTime = TICKS_CNT;
	nTimeout = TICKS_PER_SEC / 100;
	// Do a loop and wait for Pin ARB to go low.. timeout here
	while ((nLast & DQ3) == 0x00)
	{
		ccbi(PORT_AOE);
		nLast = PIN_BU1;
		ssbi(PORT_AOE);
		if (TICKS_CNT > nLast)
			nThis += TICKS_CNT - nTime;
		nTime = TICKS_CNT;
		if (nThis > nTimeout )
		{
			bStatus = FLASH_TIMEOUT;
			break;
		}
	}

#if HWVER > 2
	ccbi(PORT_AOE);
	nLast = PIN_BU1;
	ssbi(PORT_AOE);

	do
	{
		// Read Byte (DQ0-DQ7)
		ccbi(PORT_AOE);
		nThis = PIN_BU1;
		ssbi(PORT_AOE);

		if ( (nThis & DQ6) == (nLast & DQ6) ) //DQ6=Toggle?
		{
			// No = Pass
			break;
		}
		else if ( nThis & DQ5 ) // Yes, DQ5=1?
		{
			// Yes, Read (DQ0-DQ7)	
			nLast = nThis;
			ccbi(PORT_AOE);
			nThis = PIN_BU1;
			ssbi(PORT_AOE);
	
			if ( (nThis & DQ6) == (nLast & DQ6) ) // DQ6 = Toggle?
			{
				// No = Pass			
				break;
			}
			else
			{
				// Yes = Fail
				bStatus = FLASH_FAIL;
			}
		}
		
		nLast = nThis;
	} while (bStatus == FLASH_OK);


/*	while ( nLast != 0xFF && bStatus == FLASH_OK)
	{
		
		ccbi(PORT_AOE);
		nLast = PIN_BU1;
		ssbi(PORT_AOE);
		if ((nLast & DQ5) && (nLast != 0xFF))
		{
			ccbi(PORT_AOE);
			nLast = PIN_BU1;
			ssbi(PORT_AOE);

			if ((nLast != 0xFF) && (nLast & DQ5))
			{
				bStatus = FLASH_FAIL;
			}
		}
	}*/
#else
	nTime = TICKS_CNT;
	nThis = 0;
	nTimeout = TICKS_PER_SEC;
	while (bStatus == FLASH_OK && bit_is_clr(PIN_ARB))
	{
		if (TICKS_CNT > nTime)
			nThis += TICKS_CNT - nTime;
		nTime = TICKS_CNT;

		//nThis = TICKS_CNT - nTime;
		if (nThis > nTimeout )
		{
			bStatus = FLASH_FAIL;
			break;
		}
		else
		{
			Delay(10);
		}
	}
#endif

	modeRun();
	THREADS_GO;

	return bStatus;
}

void flashCopy(uint32_t* from, uint32_t* to, uint32_t* length)
{
	uint16_t	timeout = TICKS_PER_SEC / 10;
	uint16_t	bticks = 0;
	uint16_t	i;
	uint8_t		lticks = TICKS_CNT;
	uint8_t		data[2];

	cli();
	THREADS_STOP;

	// 16-bit address'
	*length = *length >> 1;
	*from	= *from >> 1;
	*to		= *to >> 1;

	modeReadWrite();
	ccbi(PORT_ACE);
#if HWVER != 31
	ccbi(PORT_AACOE);
#endif

	for (i=0;i<*length;i++)
	{
		// Reset watchdog
		wdt_reset();

		// Read in bytes
		AddressInit((_typAddress*)from);
		ccbi(PORT_AOE);
		*from = *from + 1;
		data[0] = inb(PIN_BU2);
		data[1] = inb(PIN_BU1);
		ssbi(PORT_AOE);

		if (data[0] != 0xFF || data[1] != 0xFF)
		{
			// Write bytes
			modeSendWriteCmd(); // Send the WriteCommand
			AddressInit((_typAddress*)to); // Set the Address
			outb(PORT_BU2, data[0]);			// Set the Data
			outb(PORT_BU1, data[1]);		// Set more Data
			pulseLo(AWE);						// Pulse Write
			outb(DDR_BU1,0x00);					// Set as inputs
			outb(DDR_BU2,0x00);					// Set as inputs
			ccbi(PORT_AOE);						// Read the data
			lticks = TICKS_CNT;
			bticks = 0;
			while (((inb(PIN_BU1) != data[1]) || (inb(PIN_BU2) != data[0])) && (bticks < timeout))
			{
				if (TICKS_CNT > lticks)
					bticks += ((uint8_t)inb(TICKS_CNT) - (uint8_t)lticks);
				lticks = TICKS_CNT;
			}
			if (bticks >= timeout)
			{
				THREADS_GO;
				modeRun();
				FrameResponse(FLASH_WRITE_TIMEOUT);
				
				return;
			}
			ssbi(PORT_AOE);
		}
		*to = *to + 1;
	}

	THREADS_GO;
	modeRun();

	FrameResponse(CMD_OK);
}

uint8_t flash_write(uint32_t address, unsigned char *Buffer)
{
	uint16_t	timeout = TICKS_PER_SEC / 10;
	uint16_t	bticks = 0;
	uint16_t	lticks = 0;

	if ((Buffer[0] != 0xFF ) || (Buffer[1] != 0xFF))
	{
		wdt_reset();
		modeSendWriteCmd();					// Send the WriteCommand
		AddressInit((_typAddress*)&address); // Set the Address
		outb(PORT_BU2, Buffer[0]);			// Set the Data
		outb(PORT_BU1, Buffer[1]);			
		pulseLo(AWE);						// Pulse Write
		outb(DDR_BU1,0x00);					// Set as inputs
		outb(DDR_BU2,0x00);					
		ccbi(PORT_AOE);						// Read the data
		lticks = TICKS_CNT;
		bticks = 0;

		while (((inb(PIN_BU1) != Buffer[1]) || (inb(PIN_BU2) != Buffer[0])) && (bticks < timeout))
		{
			if (TICKS_CNT > lticks)
				bticks += ((uint8_t)inb(TICKS_CNT) - (uint8_t)lticks);
			lticks = TICKS_CNT;
		}

		if (bticks >= timeout)
		{	
			return FALSE;
		}
		ssbi(PORT_AOE);
	}

	return TRUE;
}

void flashWrite(uint32_t* address, uint16_t* length, uint8_t* Buffer)
{
	uint16_t	timeout = TICKS_PER_SEC / 10;
	uint16_t	bticks = 0;
	uint16_t i;
	uint8_t		lticks = TICKS_CNT;
	
	cli();

	// Convert to 16bit address
	*address = *address >> 1;

	AddressInit((_typAddress*)address);
	//outb(DDR_BU1, 0xFF);
	//outb(DDR_BU2, 0xFF);

	modeReadWrite();

	*length &= 0xFFFE;
	THREADS_STOP;

	for (i=0;i<*length;i+=2)
	{
		if ((Buffer[i] != 0xFF ) || (Buffer[i+1] != 0xFF))
		{
			wdt_reset();
			modeSendWriteCmd(); // Send the WriteCommand
			AddressInit((_typAddress*)address); // Set the Address
			outb(PORT_BU2, Buffer[i]);			// Set the Data
			outb(PORT_BU1, Buffer[i+1]);		// Set more Data
			pulseLo(AWE);						// Pulse Write
			outb(DDR_BU1,0x00);					// Set as inputs
			outb(DDR_BU2,0x00);					// Set as inputs
			ccbi(PORT_AOE);						// Read the data
			lticks = TICKS_CNT;
			bticks = 0;
			while (((inb(PIN_BU1) != Buffer[i+1]) || (inb(PIN_BU2) != Buffer[i])) && (bticks < timeout))
			{
				if (TICKS_CNT > lticks)
					bticks += ((uint8_t)inb(TICKS_CNT) - (uint8_t)lticks);
				lticks = TICKS_CNT;
			}
			if (bticks >= timeout)
			{
				THREADS_GO;
				modeRun();
				FrameResponse(FLASH_WRITE_TIMEOUT);
				
				return;
			}
			ssbi(PORT_AOE);
			
		} 
		*address = *address + 1;
	}
	
	THREADS_GO;
	modeRun();

	FrameResponse(CMD_OK);
}

void eepromRead(uint16_t address, uint16_t* length)
{
	uint8_t	chHdr[] = { ID_EEPROM_DATA, 0,
					(uint8_t)(address >> 8),
					(uint8_t)(address ),
					(uint8_t)(*length >> 8),
					(uint8_t)(*length)};
	uint8_t	chData[*length];


#ifndef DEBUG_MODE
	address += EE_START;
#endif

	wdt_reset();

	// Read the block
	eeprom_read_block((void*)chData, (const void*)address, *length);

	// Send the response
	comm_new(eMsgPc,eMsgAtmel, *length + sizeof(chHdr));
	comm_append(chHdr, sizeof(chHdr));
	comm_append(chData, *length);
	comm_send();
}

// The Buffer is the exact start point of the buffer, no headers or footer crap
void eepromWrite(uint16_t address, uint16_t* length, uint8_t* Buffer)
{
	uint16_t i;
	// Check for out of bounds
	/*if ((address + *length) >= EE_END)
	{
	FrameResponse(CMD_FAILED);
	return;
	}*/

#ifndef DEBUG_MODE
	address += EE_START;
#endif
	cli();
	for (i=0;i<*length;i++)
	{
		if (Buffer[i] != eeprom_read_byte((void*)address+i))
		{
			eeprom_write_byte((void*)address+i, Buffer[i]);
			wdt_reset();
		}
	}

	FrameResponse(CMD_OK);
}



__attribute__((noreturn)) int main(void) 
//
{
	wdt_disable();

	comm_init(&comm_usb_handler);

	//--------------- Initialise atmel here
	int_startup();

#ifdef SIG_WATCHDOG_TIMEOUT		
	if (wdt_watchdog)
	{
		FrameResponse(WATCHDOG_RESET);
		wdt_watchdog = 0;
	}
#endif

	FrameResponse(CMD_BOOT);
	
	Initialise();
	AddressReset();

	//mGlobalData.mTraceData.Address = 0x4080;

	for(;;)
	{
#if HWVER == 31
		trace_check();
#endif 
	}
}


//------------------------------------------- Timer Functions (Threads)

/* FUNCTIOn
/	Timer := Collect CPU Usage stats
*/
void tmrCpuStats(void* typTimer)
{
	mGlobalData.mCPUInfo.mCPUStats.threadTime = ((float)mGlobalData.mCPUInfo.nTicks / (float)TICKS_PER_SEC );
	mGlobalData.mCPUInfo.nTicks = 0;
}

/* FUNCTION
/	Timer := Upload frame data to the UART
/  DESCRIPTION
/	Uploads a complete frame to the PC if in the correct mode
/  NOTES
/	This function could be implemented by adding and removing the timer function if
/	and when required.
*/
void tmrFrameUpload(void* typTimer)
{
	if (m_Frame_Stream == MAIN_FRAME_PASS)
		FrameUpload();

	if (m_Frame_Log == eRunning)
	{
		log_write_entry((uint8_t*)&mGlobalData,sizeof(mGlobalData), CPU_TICKS);
	}
}

/* FUNCTION
/	Timer := Perform AD Lookup
/  DESCRIPTION
/	Looks up all ADC Values
/  NOTES
/	This function could be implemented by adding and removing the timer function if
/	and when required.
*/
void tmrADLookup(void* typTimer)
{
	_typTimers*	Timer = (_typTimers*)typTimer;

	Timer->mFncParam = ++Timer->mFncParam % 3;

	ADlookupNum(Timer->mFncParam);
}

/* FUNCTION
/	Timer := Check consult Initialisation
/  DESCRIPTION
/	Checks if the consult is initialised
*/
void tmrConsultInit(void* typTimer)
{
	_typTimers*	Timer = (_typTimers*)typTimer;

	consult_enable(1);

	if (m_Frame_Stream != MAIN_FRAME_BYPASS)
	{
		if (consult_check_status() == 0x00)
		{
			if ((Timer->mFncParam++) > 0x04)
			{
				consult_init_comm();
				//InitialiseConsult();
				Timer->mFncParam = 0;
			}
		} else
		{
			Timer->mFncParam = 0;
		}
	}
#if HWVER == 31
	if (m_Frame_Log == eUnInit && !bit_is_clr(MMC_CD))
	{
		uint32_t	dts;
		eeprom_read_block(&dts,(const void*)EE_SD_DTS,sizeof(dts));

		switch (log_init(dts))
		{
		case eLogFail:
			m_Frame_Log = eUnInit;
			break;
		case eLogOk:
			m_Frame_Log = eRunning;
			break;
		case eLogNoSpace:
		case eLogSdRdFail:
		case eLogSdWrFail:
			m_Frame_Log = eNoInit;
			break;
		}
	}
#endif
}

unsigned char consultbyte(uint8_t position, uint32_t BitSet)
{
	uint16_t i;
	uint16_t byCount = 0;

	for(i=0;i<32;i++)
	{
		if ((BitSet >> i) & (uint32_t)0x01)
		{
			byCount++;
		}
		if ((byCount-1) == position)
		{
			return i;
		}
	}
	return 0xFF;
}

void setConsultPacket(uint8_t* buffer, uint8_t bytes, uint32_t BitSet)
{
	uint8_t ch;
	uint8_t i;

	memcpy( &mGlobalData.Consult.Packet, buffer, bytes );

	mGlobalData.Consult.BitSet = BitSet;

	if (bytes > 0) {
		for (i=0;i<bytes-2;i++)
		{
			ch = mGlobalData.Consult.Packet[i+2];
			switch (consultbyte(i,BitSet))
			{
				case CB_RPM_HI:
					mGlobalData.Consult.m_RPM = (mGlobalData.Consult.m_RPM & 0x00FF) | (uint16_t)ch << 8;
					break;
				case CB_RPM_LO:
					mGlobalData.Consult.m_RPM = (mGlobalData.Consult.m_RPM & 0xFF00) | (uint16_t)ch ;
					break;
				case CB_AFM_HI:
					mGlobalData.Consult.m_AFM= (mGlobalData.Consult.m_AFM & 0x00FF) | (uint16_t)ch << 8;
					break;
				case CB_AFM_LO:
					mGlobalData.Consult.m_AFM = (mGlobalData.Consult.m_AFM & 0xFF00) | (uint16_t)ch ;
					break;
				case CB_TEMP:
					mGlobalData.Consult.m_TEMP = ch;
					break;
				case CB_SPEED:
					mGlobalData.Consult.m_SPEED = ch;
					break;
				case CB_TPS:
					mGlobalData.Consult.m_TPS = ch;
					break;
				case CB_LOAD:
					mGlobalData.Consult.m_LOAD = ch;
					break;
				case CB_INJ_HI:
					mGlobalData.Consult.m_INJ = (mGlobalData.Consult.m_INJ & 0x00FF) | (uint16_t)ch << 8;
					break;
				case CB_INJ_LO:
					mGlobalData.Consult.m_INJ = (mGlobalData.Consult.m_INJ & 0xFF00) | (uint16_t)ch ;
					break;
				case CB_BVOLT:
					mGlobalData.Consult.m_BVOLT = ch;
					break;
				case CB_IGN:
					mGlobalData.Consult.m_IGN = ch;
					break;
			}
		}
	}
}

/* FUNCTION
/	Timer := Get inputs
/  DESCRIPTION
/	Read the input channel of the BikiRom IO
/  TO-DO
/	Move getNextADC into its own timer, average 5 readings per channel
*/
void tmrGetInputs(void* typTimer)
{
	// Gets all the input pins
	if (!bit_is_clr(PIN_USRSW1))
	{
		if (mGlobalData.SwInputs[0].bounce < 2)
			mGlobalData.SwInputs[0].bounce++;
	} else
	{
		if (mGlobalData.SwInputs[0].bounce > 0)
			mGlobalData.SwInputs[0].bounce--;
	}
	if ((mGlobalData.SwInputs[0].bounce >= 0x02) && (mGlobalData.SwInputs[0].value == 0x00))
		mGlobalData.SwInputs[0].value = 0x01;
	else if ((mGlobalData.SwInputs[0].bounce == 0x00) && (mGlobalData.SwInputs[0].value == 0x01))
		mGlobalData.SwInputs[0].value = 0x00;

	if (!bit_is_clr(PIN_USRSW2))
	{
		if (mGlobalData.SwInputs[1].bounce < 2)
			mGlobalData.SwInputs[1].bounce++;
	} else {
		if (mGlobalData.SwInputs[1].bounce > 0)
			mGlobalData.SwInputs[1].bounce--;
	}
	if ((mGlobalData.SwInputs[1].bounce >= 0x02) && (mGlobalData.SwInputs[1].value == 0x00))
		mGlobalData.SwInputs[1].value = 0x01;
	else if ((mGlobalData.SwInputs[1].bounce < 1) && (mGlobalData.SwInputs[1].value == 0x01))
		mGlobalData.SwInputs[1].value = 0x00;

	if (!bit_is_clr(PIN_USRSW3))
	{
		if (mGlobalData.SwInputs[2].bounce < 2)
			mGlobalData.SwInputs[2].bounce++;
	} else {
		if (mGlobalData.SwInputs[2].bounce > 0)
			mGlobalData.SwInputs[2].bounce--;
	}
	if ((mGlobalData.SwInputs[2].bounce >= 0x02) && (mGlobalData.SwInputs[2].value == 0x00))
		mGlobalData.SwInputs[2].value = 0x01;
	else if ((mGlobalData.SwInputs[2].bounce < 1) && (mGlobalData.SwInputs[2].value == 0x01))
		mGlobalData.SwInputs[2].value = 0x00;

	if (!bit_is_clr(PIN_USRSW4))
	{
		if (mGlobalData.SwInputs[3].bounce < 2)
			mGlobalData.SwInputs[3].bounce++;
	} else {
		if (mGlobalData.SwInputs[3].bounce > 0)
			mGlobalData.SwInputs[3].bounce--;
	}
	if ((mGlobalData.SwInputs[3].bounce >= 0x02) && (mGlobalData.SwInputs[3].value == 0x00))
		mGlobalData.SwInputs[3].value = 0x01;
	else if ((mGlobalData.SwInputs[3].bounce < 1) && (mGlobalData.SwInputs[3].value == 0x01))
		mGlobalData.SwInputs[3].value = 0x00;

	if (!bit_is_clr(PIN_USRSW5))
	{
		if (mGlobalData.SwInputs[4].bounce < 2)
			mGlobalData.SwInputs[4].bounce++;
	} else {
		if (mGlobalData.SwInputs[4].bounce > 0)
			mGlobalData.SwInputs[4].bounce--;
	}
	if ((mGlobalData.SwInputs[4].bounce >= 0x02) && (mGlobalData.SwInputs[4].value == 0x00))
		mGlobalData.SwInputs[4].value = 0x01;
	else if ((mGlobalData.SwInputs[4].bounce < 1) && (mGlobalData.SwInputs[4].value == 0x01))
		mGlobalData.SwInputs[4].value = 0x00;

	mGlobalData.SwInputs[5].value = !bit_is_clr(PIN_IGNINT);

}
/* FUNCTION
/	Timer := Get ADC
/  DESCRIPTION
/	Get ADC Inputs Timer
*/
void tmrGetAdc(void* typTimer)
{
	adc_start_sequence();
	// Rotates the ADC Mux to get the next one.
	//getNextADC();
}
void tmrTurboTimer(void* typTimer)
{
	// do some shit here
}
/* FUNCTION
/	Calculate Triggers
/  DESCRIPTION
/	Does a simple Trigger calculation
/  NOTE
/	Triggers are not calcualted on demand, they are done in order
/	so that values can override one another in a controled facion
*/
void tmrTriggerSet(void* typTimer)
{
	uint8_t State = 0x00;

	State = TriggerCalc(EE_BKT32, &State, &mGlobalData.Triggers, 31);
	State = TriggerCalc(EE_BKT31, &State, &mGlobalData.Triggers, 30);
	State = TriggerCalc(EE_BKT30, &State, &mGlobalData.Triggers, 29);
	State = TriggerCalc(EE_BKT29, &State, &mGlobalData.Triggers, 28);
	State = TriggerCalc(EE_BKT28, &State, &mGlobalData.Triggers, 27);
	State = TriggerCalc(EE_BKT27, &State, &mGlobalData.Triggers, 26);
	State = TriggerCalc(EE_BKT26, &State, &mGlobalData.Triggers, 25);
	State = TriggerCalc(EE_BKT25, &State, &mGlobalData.Triggers, 24);
	State = TriggerCalc(EE_BKT24, &State, &mGlobalData.Triggers, 23);
	State = TriggerCalc(EE_BKT23, &State, &mGlobalData.Triggers, 22);
	State = TriggerCalc(EE_BKT22, &State, &mGlobalData.Triggers, 21);
	State = TriggerCalc(EE_BKT21, &State, &mGlobalData.Triggers, 20);
	State = TriggerCalc(EE_BKT20, &State, &mGlobalData.Triggers, 19);
	State = TriggerCalc(EE_BKT19, &State, &mGlobalData.Triggers, 18);
	State = TriggerCalc(EE_BKT18, &State, &mGlobalData.Triggers, 17);
	State = TriggerCalc(EE_BKT17, &State, &mGlobalData.Triggers, 16);
	State = TriggerCalc(EE_BKT16, &State, &mGlobalData.Triggers, 15);
	State = TriggerCalc(EE_BKT15, &State, &mGlobalData.Triggers, 14);
	State = TriggerCalc(EE_BKT14, &State, &mGlobalData.Triggers, 13);
	State = TriggerCalc(EE_BKT13, &State, &mGlobalData.Triggers, 12);
	State = TriggerCalc(EE_BKT12, &State, &mGlobalData.Triggers, 11);
	State = TriggerCalc(EE_BKT11, &State, &mGlobalData.Triggers, 10);
	State = TriggerCalc(EE_BKT10, &State, &mGlobalData.Triggers, 9);
	State = TriggerCalc(EE_BKT9, &State, &mGlobalData.Triggers, 8);
	State = TriggerCalc(EE_BKT8, &State, &mGlobalData.Triggers, 7);
	State = TriggerCalc(EE_BKT7, &State, &mGlobalData.Triggers, 6);
	State = TriggerCalc(EE_BKT6, &State, &mGlobalData.Triggers, 5);
	State = TriggerCalc(EE_BKT5, &State, &mGlobalData.Triggers, 4);
	State = TriggerCalc(EE_BKT4, &State, &mGlobalData.Triggers, 3);
	State = TriggerCalc(EE_BKT3, &State, &mGlobalData.Triggers, 2);
	State = TriggerCalc(EE_BKT2, &State, &mGlobalData.Triggers, 1);
	State = TriggerCalc(EE_BKT1, &State, &mGlobalData.Triggers, 0);

	SetMap(State);
	mGlobalData.FlashMap = State;
}

// Calculate the Trigger state, given the address, its current indivudal trigger and the global
// prioritised map value select
uint8_t TriggerCalc(uint16_t Address, uint8_t* State, uint32_t* Triggers, uint8_t TrigerNum)
{
	uint8_t Polarity = (eeprom_read_byte((uint8_t*)Address+1) & 0x01) ^ 0x01;
	float VarVal = getVarVal(eeprom_read_byte((uint8_t*)Address+3));
	uint8_t	TrigState = *Triggers & (0x01 << TrigerNum) ? 0x01 : 0x00;

	// Trigger Operation
	uint8_t TrigOp = eeprom_read_byte((uint8_t*)Address+15);
	uint8_t TrigNum = TrigOp & 0x1F;
	uint8_t	TrigVal;
	TrigOp = (TrigOp & 0xE0) >> 5;

	switch (eeprom_read_byte((uint8_t*)Address+2))
	{
	case BCTT_MAPCHANGE:
		TrigState = TrigState ^ Polarity;
		OutputOnOff(Address+4, &VarVal, &TrigState);
		TrigState = TrigState ^ Polarity;

		if ((TrigNum != 0x1F) && (TrigOp != 0x07))
		{
			TrigVal = *Triggers & (0x01 << TrigNum) ? 0x01 : 0x00;
			TrigState = logicalOperation(&TrigOp, &TrigState, &TrigVal);
		}

		*Triggers |= TrigState << TrigerNum;

		if (TrigState == 1) // Polarity to switch is True, 1 Hi, and Triger is On
			return eeprom_read_byte((uint8_t*)Address);
		else
			return *State;
		break;
	case BCTT_ONOFF:
		TrigState = TrigState ^ Polarity;
		OutputOnOff(Address+4, &VarVal, &TrigState);
		TrigState = TrigState ^ Polarity;

		if ((TrigNum != 0x1F) && (TrigOp != 0x07))
		{
			TrigVal = *Triggers & (0x01 << TrigNum) ? 0x01 : 0x00;
			TrigState = logicalOperation(&TrigOp, &TrigState, &TrigVal);
		}

		*Triggers |= TrigState << TrigerNum;

		return *State;
		break;
	default:
		*Triggers &= ~(TrigState << TrigerNum);
		return *State;
	}
}
static uint8_t TriggerCheck(uint16_t Address)
{
	uint8_t Trigger;
	uint8_t TriggerPol;

	Trigger = eeprom_read_byte((void*)Address);
	TriggerPol = ((Trigger & 0x80) / 0x80);
	Trigger &= 0x7F;
	if (((Trigger < TRIGGER_COUNT) || (Trigger == 0x7F))	&&
		((Trigger == 0x7F) || (TriggerPol == ((mGlobalData.Triggers >> Trigger) & 0x01))))
	{
		return 1;
	} else {
		return 0;
	}
}


static uint8_t logicalOperation(uint8_t* Operation, uint8_t* ValOne, uint8_t* ValTwo)
{
	switch (*Operation)
	{
	case XOR:
		return (*ValOne ^ *ValTwo);
	case NXOR:
		return !(*ValOne ^ *ValTwo);
	case AND:
		return (*ValOne & *ValTwo);
	case NAND:
		return !(*ValOne & *ValTwo);
	case OR:
		return (*ValOne | *ValTwo);
	case NOR:
		return !(*ValOne | *ValTwo);
	}
	return 1;
}

/* FUNCTION
/	Output Calculatuion
/  DESCRIPTION
/	Calculates and sets the outputs for the BikiRom IO
*/
void tmrOutputsSet(void* typTimer)
{
	_typTimers*			Timer = (_typTimers*)typTimer;
	_typOutput			*Output = &mGlobalData.Outputs[Timer->mFncParam];
	_typOutputMem*		OutputMem = &mOutputMem[Timer->mFncParam];
	uint8_t				Config;

	// Remeber which output number in the memory
	OutputMem->OutputNumber = Timer->mFncParam;

	switch (Timer->mFncParam)
	{
	case 0:
		//Timer 3A
		// ------------------------ Auxilary Output AX1
		Config = OutputsCalculate(EE_BK1, Output, OutputMem);
#ifdef AUX_OUT_1_STATUS
		if ((Config == BCID_BOOSTCONTROL || Config == BCID_GENERALPWM || Config == BCID_PID) &&
		   AUX_OUT_1_STATUS)
		{
			AUX_OUT_1_SET(Output->OnOff);
			AUX_OUT_1_SET_VALUE(Output->Value);
		} else
#endif
		{
			if (Output->OnOff)
			{
				if (Output->Value >= 0.50)
					ssbi(PORT_AUX1);
				else
					ccbi(PORT_AUX1);
			} else
				ccbi(PORT_AUX1);
		}
		break;

	case 1:
		// ------------------------ Auxilary Output AX2
		Config = OutputsCalculate(EE_BK2, Output, OutputMem);
#ifdef AUX_OUT_2_STATUS
		if ((Config == BCID_BOOSTCONTROL || Config == BCID_GENERALPWM || Config == BCID_PID) &&
		   AUX_OUT_2_STATUS)
		{
			AUX_OUT_2_SET(Output->OnOff);
			AUX_OUT_2_SET_VALUE(Output->Value);
		} else
#endif
		{
			if (Output->OnOff) {
				if (Output->Value >= 0.50)
					ssbi(PORT_AUX2);
				else
					ccbi(PORT_AUX2);
			} else
				ccbi(PORT_AUX2);
		}
		break;

	case 2:
		//Timer 1C
	// ------------------------ Auxilary Output AX3
		Config = OutputsCalculate(EE_BK3, Output, OutputMem);
#ifdef AUX_OUT_3_STATUS
		if ((Config == BCID_BOOSTCONTROL || Config == BCID_GENERALPWM || Config == BCID_PID) &&
		   AUX_OUT_3_STATUS)
		{
			AUX_OUT_3_SET(Output->OnOff);
			AUX_OUT_3_SET_VALUE(Output->Value);
		} else
#endif
		{
			if (Output->OnOff) {
				if (Output->Value >= 0.50)
					ssbi(PORT_AUX3);
				else
					ccbi(PORT_AUX3);
			} else
				ccbi(PORT_AUX3);
		}
		break;
		
	case 3:
		// ------------------------ Auxilary Output AX4
		Config = OutputsCalculate(EE_BK4, Output, OutputMem);
#ifdef AUX_OUT_4_STATUS
		if ((Config == BCID_BOOSTCONTROL || Config == BCID_GENERALPWM || Config == BCID_PID) &&
		   AUX_OUT_4_STATUS)
		{
			AUX_OUT_4_SET(Output->OnOff);
			AUX_OUT_4_SET_VALUE(Output->Value);
		} else
#endif
		{
			if (Output->OnOff) {
				if (Output->Value >= 0.50)
					ssbi(PORT_AUX4);
				else
					ccbi(PORT_AUX4);
			} else
				ccbi(PORT_AUX4);
		}
		break;
#ifdef AUX_OUT_5_STATUS
	case 4:
		// ------------------------ Digital to Analogue Output 1
		Output->Value = calcDAC(&Output->OnOff);
		// If DAC is enabled, output the shit
		if ( eeprom_read_byte((void*)EE_DAC_OE) != 0xFF )
		{
			if (AUX_OUT_5_STATUS && Output->OnOff)
			{
				AUX_OUT_5_SET(Output->OnOff);
				AUX_OUT_5_SET_VALUE(Output->Value);
			}
			else
				AUX_OUT_5_SET(Output->OnOff);
		}
		break;
#endif
	}

	// Increment the Paramater to the next output
	Timer->mFncParam = ++Timer->mFncParam % 5;
}

// Takes an eeprom pointer address for the BikiRom Ouputs config shit
uint8_t OutputsCalculate(uint16_t Address, struct _typOutput *Output, struct _typOutputMem *State)
{
	uint8_t		i;
	uint8_t		Config = eeprom_read_byte((uint8_t*)Address++);
	uint16_t	Offset = 0;
	uint16_t	Value, Value2;

//	if (SecurityUnlockCheck(Config))
//	{
		switch (Config)
		{
		case BCID_PID:
			{
				// Load the PID state machine
				struct SPid	*PID = (struct SPid*)&State->Data;

				// check if intiialised. If not, lead data structure
				if (!State->Initialised) 
				{
					// Read in the structue from the EEPROM
					struct _typPID	Load;
					eeprom_read_block((void*)&Load, (void*)Address, sizeof(struct _typPID));

					PID->overValue = 1;
					PID->overTime = 0;
					PID->underValue = 0;
					PID->underTime = 0;

					PID->dGain = Load.dGain;
					PID->iGain = Load.iGain;
					PID->iMax = Load.iMax;
					PID->iMin = Load.iMin;
					PID->pGain = Load.pGain;
					PID->dState = 0;
					PID->iState = 0;
					PID->Position = Load.Position;
					PID->Positive = Load.Positive;
					PID->dError = PID->Position - getVarValReal(PID->VarValue, EE_ALU_UNIT);
					PID->VarValue = Load.varValue;
					PID->tGain = Load.tGain;
					PID->minTime = Load.MinTime / TIMERSCHEDULEMS;
					Output->Value = Load.iValue;
					State->Initialised = TRUE;
				}

				// Do the calculation
				if (PID->Positive) 
				{
					Output->Value += UpdatePID(PID, Output->Value, &TICKS, PID->Position - getVarValReal(PID->VarValue, EE_ALU_UNIT), PID->Position);
				} else
				{
					Output->Value -= UpdatePID(PID, Output->Value, &TICKS, PID->Position - getVarValReal(PID->VarValue, EE_ALU_UNIT), PID->Position);
				}

				Output->OnOff = On;

				if (Output->Value > 1.00)
					Output->Value = 1.00;
				else if (Output->Value < 0)
					Output->Value = 0.00;
			}
			break;
		case BCID_BOOSTCONTROL:
		case BCID_GENERALPWM:
		case BCID_GENERALONOFF:
			{
				float		TrimPer = 0.50;
				float		TrimVal = 0.00;
				// Always initialy On unless otherwise
				float		BaseVal = 1.00;
				float		VarMin = 0.00;
				float		VarMax = 0.00;
				float		VarVal = 0.00;
				// Parse all the sub tables, values, ranges etc and sumaries them in Per, Val, Base
				Output->OnOff = 1;
				for (i=0;i<9;i++)
				{
					Offset = Address + (i*28);
					if (eeprom_read_byte((void*)Offset) != BCID_UNCONFIGURED)
					{
						if (TriggerCheck(Offset+22))
						{
							if (eeprom_read_byte((void*)Offset) != BCTT_ONOFF)
							{
								VarMin = getFloatS(Offset+2);
								VarMax = getFloatS(Offset+23);

								if (VarMin > VarMax)
									break;
								VarVal = getVarVal(eeprom_read_byte((void*)Offset+1));
								switch (eeprom_read_byte((void*)Offset))
								{
								case BCTT_TRIMPER:
									TrimPer += OutputLookup(Offset+6, &VarMin, &VarMax, &VarVal) - (float)0.50;
									break;
								case BCTT_TRIMVAL:
									TrimVal += (OutputLookup(Offset+6, &VarMin, &VarMax, &VarVal) - (float)0.50) * 2;
									break;
								case BCTT_BASE:
									BaseVal = OutputLookup(Offset+6, &VarMin, &VarMax, &VarVal);
									break;
								}
							} else // BCTT_ONOFF
							{
								OutputOnOff(Offset+2, &VarVal, &Output->OnOff);
							}
						}
					}
				}
				
				//Output->Value = BaseVal;
				Output->Value = ((TrimVal + BaseVal) * TrimPer ) / (float)0.50;
				if (Output->Value > 1.00)
					Output->Value = 1.00;
				else if (Output->Value < 0)
					Output->Value = 0.00;
			}
			break;
		case BCID_TURBOTIMER:
			if (!State->Initialised)
			{
				// Initialise the Turbo Time Output interrupts
				ptypTurboTimer = (_typTurboTimer*)&State->Data;
				
				// Initialise the varialbes
				ptypTurboTimer->eLogicState	= eTTWait;
				ptypTurboTimer->nAuxChannel = State->OutputNumber;
				ptypTurboTimer->nTimeMin	= (uint16_t)eeprom_read_byte((void*)Address) << 8 | eeprom_read_byte((void*)Address+1);
					//eeprom_read_word((void*)Address);
				ptypTurboTimer->nTimeMax	= (uint16_t)eeprom_read_byte((void*)Address+20) << 8 | eeprom_read_byte((void*)Address+21);
					//eeprom_read_word((void*)Address+20);

				// Attach the Interrupt
				int_attach(INT_IGN, eIntCtrlFalling, &OutputTurboTimer);

				// Set states to On and Initialised
				Output->OnOff = 0x01;
				State->Initialised = 0x01;
			}
			else
			{
				switch (ptypTurboTimer->eLogicState )
				{
				case eTTPreOn:			// Enable Minimum time ON
					BikTimerInit(&tmrOutputTurboTimer, eTTPreTriggers, ptypTurboTimer->nTimeMin * 1000, 0, FALSE,1);
					ptypTurboTimer->eLogicState = eTTOn;
				case eTTOn:				// Keep Output ON
					Output->Value = 1;
					break;

				case eTTPreTriggers:	// Enable Maximum time ON
					BikTimerInit(&tmrOutputTurboTimer, eTTPreOff, ptypTurboTimer->nTimeMax * 1000, 0, FALSE,1);
					ptypTurboTimer->eLogicState = eTTTriggers;
				case eTTTriggers:
					Output->Value = 1;

					Value = ptypTurboTimer->nTimeMin;

					// Check for the Expiration of Triggers for their time
					for (i=0;i<6;i++)
					{	
						Offset = Address + (i*3) + 2;
						if (TriggerCheck(eeprom_read_byte((void*)Offset)))
						{
							Value2 = (uint16_t)eeprom_read_byte((void*)Offset+1) << 8 | eeprom_read_byte((void*)Offset+2);
								//eeprom_read_word((void*)Offset+1);

							if (Value2 == 0)						// Special Case , Turn off Immediatley!
							{
								Value = 0;
							}
							else if (Value2 > Value 
							&& Value2 < ptypTurboTimer->nTimeMax)	// See if there is a new maximum time
							{
								Value = Value2;
							}
						}
					}
					
					// Ticks is in per 10ms, Value is in seconds
					if (((mGlobalData.mCPUInfo.nTicksMS - ptypTurboTimer->nTimeStart) / TIMERSCHEDULEHZ) > Value)
					{
						// Turn the output off
						Output->Value = 0;
						// Enter the Pre-Off mode if value turned off
						ptypTurboTimer->eLogicState = eTTPreOff;
					}
					break;

				case eTTPreOff:			// Enable Re-enable timeout
					BikTimerInit(&tmrOutputTurboTimer, eTTWait, 10000, 0, FALSE,1);
					ptypTurboTimer->eLogicState = eTTOff;
				case eTTOff:
					Output->Value = 0;
					break;

				default:
					break;
				}
			}

			/*
			if (Output->OnOff == 0x00)
			{
				Value = (uint16_t)eeprom_read_byte((void*)Address) << 8 | eeprom_read_byte((void*)Address+1);
				Address += 2;
				// Loop thru the Triggers, find ones that are on, if on get their timeout value
				for (i=0;i<6;i++)
				{
					Offset = Address + (i*3);
					if (!TriggerCheck(Offset))
						break;
					Value2 = (uint16_t)eeprom_read_byte((void*)Offset+1) << 8 | eeprom_read_byte((void*)Offset+2);
					if (Value2 > Value)
						Value = Value2;
				}
				BikTimerInit(&tmrTurboTimer, Address, Value, 0,	FALSE, 1);

				Output->OnOff = 0x01;
				Output->Value = Value;
			} */
			break;
		case BCID_ALU:
			Output->OnOff = 0x01;
			Output->Value = ALUlookup(0, Address);

			break;
		default:
			Output->OnOff = 0x00;
			Output->Value = 0.00;
			break;
		}
//	}

	return Config;
}

float getVarValReal(uint8_t Val, uint16_t AUOffset)
{
	// Trigger look up!
	if ((Val >= BCTV_TRIG1) && (Val <= BCTV_TRIG32))
	{
		return mGlobalData.Triggers & (0x01 << (Val - BCTV_TRIG1)) ? 0x01 : 0x00;
	} else if ((Val >= BCTV_AL1) && (Val <= BCTV_AL64))
		// AL Unit lookup!
	{
		return ALUlookup(Val - BCTV_AL1, AUOffset);
	} else
	{
		switch (Val)
		{
		case BCTV_RPM :
			return (float)mGlobalData.Consult.m_RPM * 12.5;
			break;
		case BCTV_AFM :
			return (float)mGlobalData.Consult.m_AFM*5;
			break;
		case BCTV_TPS :
			return (float)mGlobalData.Consult.m_TPS*20;
			break;
		case BCTV_MAP :
			return (float)mGlobalData.Consult.m_MAP;
			break;
		case BCTV_TEMP :
			return (float)mGlobalData.Consult.m_TEMP-50;
			break;
		case BCTV_SPEED :
			return (float)mGlobalData.Consult.m_SPEED*2;
			break;
		case BCTV_BKI1 :
			return (float)mGlobalData.SwInputs[0].value;
			break;
		case BCTV_BKI2 :
			return (float)mGlobalData.SwInputs[1].value;
			break;
		case BCTV_BKI3 :
			return (float)mGlobalData.SwInputs[2].value;
			break;
		case BCTV_BKI4 :
			return (float)mGlobalData.SwInputs[3].value;
			break;
		case BCTV_BKI5 :
			return (float)mGlobalData.SwInputs[4].value;
			break;
		case BCTV_BKAD1:
			return (float)mGlobalData.AdInputs[0].Raw / ADC_MULT;
			break;
		case BCTV_BKAD2:
			return (float)mGlobalData.AdInputs[1].Raw / ADC_MULT;
			break;
		case BCTV_BKAD3:
			return (float)mGlobalData.AdInputs[2].Raw / ADC_MULT;
			break;
#if HWVER == 31
		case BCTV_BKAD4:
			return (float)mGlobalData.SwAdcInputs.value[0] / ADC_MULT;
			break;
		case BCTV_BKAD5:
			return (float)mGlobalData.SwAdcInputs.value[1] / ADC_MULT;
			break;
		case BCTV_BKAD6:
			return (float)mGlobalData.SwAdcInputs.value[2] / ADC_MULT;
			break;
		case BCTV_BKAD7:
			return (float)mGlobalData.SwAdcInputs.value[3] / ADC_MULT;
			break;
		case BCTV_BKAD8:
			return (float)mGlobalData.SwAdcInputs.value[4] / ADC_MULT;
			break;
#endif

		case BCTV_BKADLU1:
			return mGlobalData.AdInputs[0].Lookup;
			break;
		case BCTV_BKADLU2:
			return mGlobalData.AdInputs[1].Lookup;
			break;
		case BCTV_BKADLU3:
			return mGlobalData.AdInputs[2].Lookup;
			break;
		case BCTV_BKAX5:
			return mGlobalData.Outputs[4].Value;
			break;
		case BCTV_BKAX4:
			return mGlobalData.Outputs[3].Value;
			break;
		case BCTV_BKAX3:
			return mGlobalData.Outputs[2].Value;
			break;
		case BCTV_BKAX2:
			return mGlobalData.Outputs[1].Value;
			break;
		case BCTV_BKAX1:
			return mGlobalData.Outputs[0].Value;
			break;
		default:
			return 0.00;
			break;
		}
	}
}
/*
	FUNCTION: getVarVal(ValueID)
	DESC: get the variable value given the variable ID
	Note: ALU lookup values are taken from the default EE ALU UNIT stack
*/
float getVarVal(uint8_t Val)
{
	// Trigger look up!
	if ((Val >= BCTV_TRIG1) && (Val <= BCTV_TRIG32))
	{
		return mGlobalData.Triggers & (0x01 << (Val - BCTV_TRIG1)) ? 0x01 : 0x00;
	} 
	else if ((Val >= BCTV_AL1) && (Val <= BCTV_AL64))
		// AL Unit lookup!
	{
		return ALUlookup(Val - BCTV_AL1, EE_ALU_UNIT);
	} 
	else
	{
		switch (Val)
		{
		case BCTV_RPM :
			return (float)mGlobalData.Consult.m_RPM;
			break;
		case BCTV_AFM :
			return (float)mGlobalData.Consult.m_AFM;
			break;
		case BCTV_TPS :
			return (float)mGlobalData.Consult.m_TPS;
			break;
		case BCTV_MAP :
			return (float)mGlobalData.Consult.m_MAP;
			break;
		case BCTV_TEMP :
			return (float)mGlobalData.Consult.m_TEMP;
			break;
		case BCTV_SPEED :
			return (float)mGlobalData.Consult.m_SPEED;
			break;
		case BCTV_BKI1 :
			return (float)mGlobalData.SwInputs[0].value;
			break;
		case BCTV_BKI2 :
			return (float)mGlobalData.SwInputs[1].value;
			break;
		case BCTV_BKI3 :
			return (float)mGlobalData.SwInputs[2].value;
			break;
		case BCTV_BKI4 :
			return (float)mGlobalData.SwInputs[3].value;
			break;
		case BCTV_BKI5 :
			return (float)mGlobalData.SwInputs[4].value;
			break;
		case BCTV_BKI6:
			return (float)mGlobalData.SwInputs[5].value;
			break;
		case BCTV_BKAD1:
			return (float)mGlobalData.AdInputs[0].Raw / 63.9375;
			break;
		case BCTV_BKAD2:
			return (float)mGlobalData.AdInputs[1].Raw / 63.9375;
			break;
		case BCTV_BKAD3:
			return (float)mGlobalData.AdInputs[2].Raw / 63.9375;
			break;

#if HWVER == 31
		case BCTV_BKI7:
			return (float)mGlobalData.SwAdcInputs.OnOff[0];
			break;
		case BCTV_BKI8:
			return (float)mGlobalData.SwAdcInputs.OnOff[1];
			break;
		case BCTV_BKAD4:
			return (float)mGlobalData.SwAdcInputs.value[0] / 64.0;
			break;
		case BCTV_BKAD5:
			return (float)mGlobalData.SwAdcInputs.value[1] / 64.0;
			break;
		case BCTV_BKAD6:
			return (float)mGlobalData.SwAdcInputs.value[2] / 64.0;
			break;
		case BCTV_BKAD7:
			return (float)mGlobalData.SwAdcInputs.value[3] / 64.0;
			break;
		case BCTV_BKAD8:
			return (float)mGlobalData.SwAdcInputs.value[4] / 64.0;
			break;
#endif

		case BCTV_BKADLU1:
			return mGlobalData.AdInputs[0].Lookup;
			break;
		case BCTV_BKADLU2:
			return mGlobalData.AdInputs[1].Lookup;
			break;
		case BCTV_BKADLU3:
			return mGlobalData.AdInputs[2].Lookup;
			break;
		case BCTV_BKAX5:
			return mGlobalData.Outputs[4].Value;
			break;
		case BCTV_BKAX4:
			return mGlobalData.Outputs[3].Value;
			break;
		case BCTV_BKAX3:
			return mGlobalData.Outputs[2].Value;
			break;
		case BCTV_BKAX2:
			return mGlobalData.Outputs[1].Value;
			break;
		case BCTV_BKAX1:
			return mGlobalData.Outputs[0].Value;
			break;
		default:
			return 0.00;
			break;
		}
	}
}


uint16_t getShort(uint16_t Address, uint16_t Offset)
{
	return (uint16_t)(eeprom_read_byte((void*)Address + (Offset * 2)) << 8) | (uint16_t)eeprom_read_byte((void*)Address + (Offset * 2) + 1);
}
/*uint16_t getShort(uint16_t Address,uint16_t Offset)
{
	return ((uint16_t)(eeprom_read_byte((void*)Address + (uint16_t)(Offset * 2)) << 8) | eeprom_read_byte((void*)Address + (uint16_t)(Offset * 2) + 1));
}*/

// assumes 16 bytes from the address provided
void OutputOnOff(uint16_t Address, float* Value, uint8_t* State)
{
	uint8_t TriggerType = eeprom_read_byte((uint8_t*)Address);

	float ValOne = getFloat(Address+1,0);
	//uint16_t ValOne = (eeprom_read_byte((void*)Address+1) << 8) | eeprom_read_byte((void*)Address+2);
	uint8_t ActionOne = eeprom_read_byte((uint8_t*)Address+5);

	float ValTwo = getFloat(Address+6,0);
	//uint16_t ValTwo = (eeprom_read_byte((void*)Address+4) << 8) | eeprom_read_byte((void*)Address+5);
	uint8_t ActionTwo = eeprom_read_byte((uint8_t*)Address+10);

	switch (TriggerType)
	{
	case BCTT_HEX:
		if (*State > 0x00) // currently on. look for off
		{
			if (Compare(Value, &ValTwo, &ActionTwo) == 0x01)
				*State = 0x00;
		} else
		{
			if (Compare(Value, &ValOne, &ActionOne) == 0x01)
				*State = 0x01;
		}
		break;
	case BCTT_RANGE:
		if ((Compare(Value, &ValOne, &ActionOne) == 0x01) && (Compare(Value, &ValTwo, &ActionTwo) == 0x01))
			*State = 0x01;
		else
			*State = 0x00;
		break;
	default:
		return;
	}
}

// Does a comparison between two values given the operator
uint8_t Compare(float *ValueOne, float *ValueTwo, uint8_t *Comparison)
{
	switch (*Comparison)
	{
	case EQ:
		if (*ValueOne == *ValueTwo)
			return 1;
		else
			return FALSE;
		break;
	case GTE:
		if (*ValueOne >= *ValueTwo)
			return 1;
		else
			return FALSE;
		break;
	case LTE:
		if (*ValueOne <= *ValueTwo)
			return 1;
		else
			return FALSE;
		break;
	case LT:
		if (*ValueOne < *ValueTwo)
			return 1;
		else
			return FALSE;
		break;
	case GT:
		if (*ValueOne > *ValueTwo)
			return 1;
		else
			return FALSE;
		break;
	case NE:
		if (*ValueOne != *ValueTwo)
			return 1;
		else
			return FALSE;
		break;
	}
	return FALSE;
}

// assumes 16 bytes from the address provided
// Does an eeprom Lookup based on the values parsed to it.
float OutputLookup(uint16_t Address, float* Min, float* Max, float* Value)
{
	uint8_t Pt = 0;
	float Split = 0;
	float Amt = 15;

	if (*Min == *Max)
	{
		return (float)eeprom_read_byte((void*)Address)/ (float)255.00;
	}
	else if (*Value <= *Min)
	{
		return (float)eeprom_read_byte((void*)Address)/ (float)255.00;
	}
	else if(*Value >= *Max)
	{
		return (float)eeprom_read_byte((void*)Address+15)/ (float)255.00;
	}
	else
	{
		Split = (*Value - *Min) / ((*Max - *Min) / Amt);
		Pt = (uint8_t)Split;
		Split = Split - Pt;

		if (Pt >= 0x0F)
		{ return (float)eeprom_read_byte((void*)Address+15) / (float)255.00; }
		else if (Split == 0x00)
		{ return (float)eeprom_read_byte((void*)Address+Pt) / (float)255.00; }
		else if (Split >= 0x01)
		{ return (float)eeprom_read_byte((void*)Address+Pt+1) / (float)255.00; }

		return ( ( Split * (float)eeprom_read_byte((void*)Address+Pt+1) ) + ( ((float)1 - Split) * (float)eeprom_read_byte((void*)Address+Pt) ) ) / (float)255.00;
	}
}

uint16_t OutputLookupWord(uint16_t Address, float Min, float Max, float* Value, float Points)
{
	uint8_t Pt = 0;
	float Split = 0;

	if (Min == Max)
	{
		return getShort(Address, 0) ;
	}
	else if (*Value <= Min)
	{
		return getShort(Address, 0) ;
	}
	else if(*Value >= Max)
	{
		return getShort(Address, (uint16_t)(Points-1)) ;
	}
	else
	{
		Split = ((*Value - Min) / (Max - Min)) * (Points-1);
		Pt = (uint8_t)Split ;
		Split = Split - Pt;

		if (Pt >= (Points-1))
		{ return getShort(Address, Points-1) ; }
		else if (Split == 0x00)
		{ return getShort(Address, Pt); }
		else if (Split >= 0x01)
		{ return getShort(Address, Pt+1); }

		return Split*(float)getShort(Address, Pt+1) + (((float)1-Split)*(float)getShort(Address, Pt) );
	}
}

float getFloat(uint16_t Address, uint8_t Num)
{
	uint8_t pt[4];
	eeprom_read_block((void*)&pt[0],(const void*)Address+(Num * 0x4), 0x04);
	return *(float*)&pt[0];
	//return arrayToFloat(&pt[0]);
}

float getFloatS(uint16_t Address)
{
	uint8_t pt[4];
	eeprom_read_block((void*)&pt[0],(const void*)Address, 0x04);
	return *(float*)&pt[0];
	//return arrayToFloat(&pt[0]);
}

static uint8_t	adcLookupEnable = 0xFF;

void ADlookupCheck(void)
{
	uint16_t	adrs[3] = { EE_LOOKUP_A, EE_LOOKUP_B, EE_LOOKUP_C };
	uint16_t	cnt, i, sts;

	for (cnt=0;cnt<3;cnt++)
	{
		sts = FALSE;
	
		i = 0xFF;
		do
		{
			if (eeprom_read_byte((void*)(adrs[cnt]+i)) != 0xFF)
			{
				sts = TRUE;
			}	
		} while (i-- && !sts);

		if (!sts)
		{
			adcLookupEnable &= ~(1<<cnt);
		}
		else
		{
			adcLookupEnable |= 1<<cnt;
		}
	}
}

void ADlookupNum(uint8_t Num)
{
	if (adcLookupEnable & (1<<Num))
	{
		switch (Num)
		{
		case 0:
			mGlobalData.AdInputs[0].Lookup = ADlookup(EE_LOOKUP_A, mGlobalData.AdInputs[0].Raw);
			break;
		case 1:
			mGlobalData.AdInputs[1].Lookup = ADlookup(EE_LOOKUP_B, mGlobalData.AdInputs[1].Raw);
			break;
		case 2:
			mGlobalData.AdInputs[2].Lookup = ADlookup(EE_LOOKUP_C, mGlobalData.AdInputs[2].Raw);
			break;
		}
	}
	else
	{
		mGlobalData.AdInputs[Num].Lookup =0;
	}
}

// Do the special 32 Word lookup for AD inputs
float ADlookup(uint16_t Address, uint16_t Value)
{
	unsigned char Pt;
	float Split;

	// 0-5040mV
	// 63 cells of 80mV per increment

#define AD_LOOKUP_MAX	(0xFFC0

	if (Value >= 0xFBC1)
	{
		return getFloat(Address, 0x3F);
	} else if (Value < 0x0001)
	{
		return getFloat(Address, 0x00);
	}
	else
	{
		Split = (float)Value / 1023;
		Pt = (uint8_t)Split;
		Split -= Pt;
		if (Pt >= 0x3F)
			return getFloat(Address, 0x3F);
		else if (Split == 0x00)
			return getFloat(Address, Pt);

		return ((Split * getFloat(Address,Pt+1)) + (((float)1 - Split) * getFloat(Address,Pt)));
	}
}

// Function caclulates the single DAC and returns a 16bit value for the 16bit PWM circuit
float calcDAC (uint8_t *OnOff)
{
	float valueBase = 0;

	if (SecurityUnlockCheck(BCID_DAC))
	{
		*OnOff = 1;
		valueBase = ALUlookup(0, EE_ALU_UNIT);
	}
	else
	{
		*OnOff = 0;
	}

	return valueBase;
	//return (float)OutputLookupWord(EE_ALU_LU, 0.00, 5040.00, &valueBase, 64);
}



float ALUlookup(uint8_t aluNum, uint16_t AUOffset)
{
	struct _typAUnit typAUnit;

	eeprom_read_block((void*)&typAUnit,
			(void*)(AUOffset + (aluNum * sizeof(typAUnit))), 
			sizeof(typAUnit));

	if ( typAUnit.Operator != 0x3F )
	{
		if (!typAUnit.typBase )
			typAUnit.Base.value = getVarValReal(typAUnit.Base.varValue, AUOffset);
		if (!typAUnit.typComp )
			typAUnit.Component.value = getVarValReal(typAUnit.Component.varValue, AUOffset);
	
		return ALUOperate(&typAUnit.Base.value, &typAUnit.Component.value, typAUnit.Operator);
	}

	return 0;
}

float ALUOperate(float* valBase, float* valOperator, uint8_t Operator)
{
	if ( Operator == MULTIPLY ) // Multiply value
	{
		return (*valBase * *valOperator);
	}
	else if (Operator == ADDITION ) // Add to value
	{
		return (*valBase + *valOperator);
	}
	else if (Operator == POWEROF ) // Exponential value
	{
		float value = 1;
		uint8_t i = 0;
		if ( *valOperator >= 0x00 )
		{
			for (i=0;i<*valOperator;i++)
			{
				value *= *valBase;
			}
			return value;
		} else
		{
			return *valBase;
		}
	} else if (Operator == SUBTRACT )
	{
		return (*valBase - *valOperator);
	}
	else if (Operator == DIVIDE )
	{
		if ( *valOperator >= 0x00 )
		{
			return ( *valBase / *valOperator ) ;
		}
		else
		{
			return 0;
		}
	}
	else // Should never get here, incase return value passed
	{
		return *valBase;
	}
}


void setMAD(uint8_t num, uint16_t* value)
{	
	mGlobalData.AdInputs[num].Raw = *value;
}

void setMAD2(uint8_t num, uint16_t* value)
{	
	mGlobalData.SwAdcInputs.value[num-AD_INPUTS_CNT] = *value;
}

void setSw2(uint8_t num, uint8_t value)
{
	mGlobalData.SwAdcInputs.OnOff[num-SWITCH_CNT] = value;
}

void setMADLU(uint8_t num, float* value)
{	mGlobalData.AdInputs[num].Lookup = *value;}

void setSw(uint8_t num, uint8_t value)
{
	if (num < SWITCH_CNT)
	{
		mGlobalData.SwInputs[num].value = value;
	}
	else
	{
		// save to new location
	}
}


// Diagnoistcs

uint8_t	diagWritePattern(void)
{
	uint32_t	nAdr = 0;
	uint8_t		bStatus = TRUE;

	THREADS_STOP;

	modeReadWrite();
	ccbi(PORT_ACE);
#if HWVER != 31
	ccbi(PORT_AACOE);
#endif

#if HWVER == 31
	nAdr = 0;

	AddressInit((_typAddress*)&nAdr);

	Delay(1);

	for	(nAdr = 0;nAdr < (FLASH_END>>2)-1 && bStatus; nAdr+= 2)
	{
		if (flash_write(nAdr, (uint8_t*)&nAdr) == FALSE
		||	flash_write(nAdr+1, (uint8_t*)&nAdr+2) == FALSE)
		{
			bStatus = FALSE;
		}
	}

	nAdr = (FLASH_END>>2)+1;
	AddressInit((_typAddress*)&nAdr);

	Delay(1);

	for	(nAdr = (FLASH_END>>2)+1;nAdr < (FLASH_END>>1)-1 && bStatus; nAdr+= 2)
	{
		if (flash_write(nAdr, (uint8_t*)&nAdr) == FALSE
		||	flash_write(nAdr+1, (uint8_t*)&nAdr+2) == FALSE)
		{
			bStatus = FALSE;
		}
	}

#endif

	modeRun();

	THREADS_GO;

	return bStatus;
}

// Run Rx to Tx loopback test
uint8_t diagRxTx(void)
{
	return consult_test_RxTx();
}
// Run Rx to CLK loopback test
uint8_t diagRxClk(void)
{
	return consult_test_RxClk();
}

// Run Flash Memory Diagnostics
uint8_t diagFlashMem(uint8_t Bank)
{
	cli();
	uint16_t i;
	uint32_t Adr = 0x01;

#if HWVER == 3 || HWVER == 31

	Adr |= (Bank * 0x080000);
	if (Bank==0x01)
		ssbi(PORT_BNKSEL);
	else
		ccbi(PORT_BNKSEL);
#endif

	modeReadWrite();

	outb(DDR_BU1, 0xFF);
	outb(DDR_BU2, 0xFF);

	// set ACE Lo
	ccbi(PORT_ACE);
	ssbi(DDR_ACE);

	// Set AOE Hi
	ssbi(PORT_AOE);
	ssbi(DDR_AOE);

#if HWVER != 31
	// send 0x5555 & 0xAA
	ssbi(PORT_AACOE);
	ssbi(PORT_CMDEV);
	ccbi(PORT_CMDOD);
	ccbi(PORT_CMDOE);
	outb(PORT_BU2, 0x00);
	outb(PORT_BU1, 0xAA);
	pulseLo(AWE);

	// send 0x2AAA & 0x55
	ccbi(PORT_CMDEV);
	ssbi(PORT_CMDOD);
	outb(PORT_BU1, 0x55);
	pulseLo(AWE);

	// send 0x5555 & 0x90
	ssbi(PORT_CMDEV);
	ccbi(PORT_CMDOD);
	outb(PORT_BU1, 0x90);
	pulseLo(AWE);

	ssbi(PORT_CMDOE);

	// read 0x0001 
	AddressInit((_typAddress*)&Adr);
	ccbi(PORT_AACOE);
#else
	// send 0x5555 & 0xAA
	PORT_ADR_55;
	//PORT_ADR_SET_M(0x05);
	//PORT_ADR_SET_L(0x55);
	outb(PORT_BU2, 0x00);
	outb(PORT_BU1, 0xAA);
	//ccbi(PORT_AWE); ccbi(PORT_AWE);
	//ssbi(PORT_AWE);
	pulseLo(AWE);

	// send 0x2AAA & 0x55
	PORT_ADR_AA;
	//PORT_ADR_SET_M(0x02);
	//PORT_ADR_SET_L(0xAA);
	outb(PORT_BU1, 0x55);
	//ccbi(PORT_AWE); ccbi(PORT_AWE);
	//ssbi(PORT_AWE);
	pulseLo(AWE);

	// send 0x5555 & 0x90
	PORT_ADR_55;
	//PORT_ADR_SET_M(0x05);
	//PORT_ADR_SET_L(0x55);
	outb(PORT_BU1, 0x90);
	//ccbi(PORT_AWE); ccbi(PORT_AWE);
	//ssbi(PORT_AWE);
	pulseLo(AWE);

	// sent 0x0001 & 0x00
	AddressInit((_typAddress*)&Adr);
#endif
	outb(DDR_BU1, 0x00);
	outb(DDR_BU2, 0x00);
	ccbi(PORT_AOE);
	ccbi(PORT_AOE);
	i = (uint16_t)inb(PIN_BU2) << 8;
	i |= inb(PIN_BU1);
	ssbi(PORT_AOE);

	modeRun();
	sei();

	switch (i)
	{
	case 0x00D3:
	case 0x22D6:
	case 0x2258:
		return 1;
	default:
		return 0;
	}
}


#ifdef EMUTEST
// RPM Output Emulator
//
#warning "Eumulator Test Enabled"

uint8_t		Sync = 0;
uint8_t	Count = 0;

void EmulatorRPMInitialise(void)
{
	//timerAttach(TIMER1OVERFLOW_INT, 
	timer_attach(TMR_OVER,&EmulatorRPMSet);

	mEmuTest.RPM		= 1000;
	mEmuTest.Cylinders	= 4;
	mEmuTest.Angles		= 90.0f;
	mEmuTest.AFM		= 0.10f;
	mEmuTest.TPS		= 0.05f;

	AUX_OUT_1_SET(2);
	AUX_OUT_2_SET(2);
	AUX_OUT_3_SET(2);
	AUX_OUT_4_SET(2);

	EmulatorRPMSet();

	ssbi(DDR_AUX1);
	ssbi(DDR_AUX2);
	ssbi(DDR_AUX3);
	ssbi(DDR_AUX4);
}

void EmulatorRPMSet(void)
{
	float pulses;

	if (Sync == 0)
		pulses=3.5f;
	else
		pulses=0.5f;

	timer_initialise_mode_exact(AUX_OUT_1_TIMER,TIMER_CLK_DIV8,mEmuTest.REF-1,TIMER_PWM);
	timer_initialise_mode_exact(AUX_OUT_3_TIMER,TIMER_CLK_DIV8,mEmuTest.SYNC-1,TIMER_PWM);

	AUX_OUT_1_SET_VALUE(0.50);
	AUX_OUT_2_SET_VALUE(mEmuTest.AFM);
	AUX_OUT_3_SET_VALUE(pulses/mEmuTest.Angles);
	AUX_OUT_4_SET_VALUE(mEmuTest.TPS);

	Sync = ++Sync % mEmuTest.Cylinders;
}

void tmrEmulator(void* typTimer)
{
	_typOutput			*Output;

	//mEmuTest.RPM = getVarValReal(BCTV_BKAD1,NULL) * 2;
#define RPM_CONST	(F_CPU / 16)

	mEmuTest.REF	= (RPM_CONST / mEmuTest.RPM) * 2;

	if ( mEmuTest.REF * mEmuTest.Angles > 0xFFFF)
	{
		mEmuTest.REF = 0xFFFF / mEmuTest.Angles;
	}

	mEmuTest.SYNC	= mEmuTest.REF * mEmuTest.Angles;
}
#endif

// Watchdog Timer reset!
#ifdef SIG_WATCHDOG_TIMEOUT

void wdt_init(void) __attribute__((naked)) __attribute__((section(".init3")));

// Function Implementation
void wdt_init(void)
{
	wdt_watchdog = 1;
    MCUSR = 0;
    wdt_disable();

    return;
}
#endif


// Turbo Timer Timeout to State Routine
void tmrOutputTurboTimer(void* typTimer)
{
	_typTimers*			Timer = (_typTimers*)typTimer;
	
	if (ptypTurboTimer != NULL)
	{
		// Set the new State for the turbo timer
		ptypTurboTimer->eLogicState = Timer->mFncParam;
	}
}

// Turbo Timer Interrupt Routine
void OutputTurboTimer(unsigned char state)
{
	// Check to see if we can configured the output or not
	if (ptypTurboTimer != NULL 
	&& (ptypTurboTimer->eLogicState == eTTWait))
	{
		ptypTurboTimer->eLogicState = eTTPreOn;
		ptypTurboTimer->nTimeStart	= mGlobalData.mCPUInfo.nTicksMS;

		// Turn the output on immediatley
		switch (ptypTurboTimer->nAuxChannel)
		{
		case 0:
			ssbi(PORT_AUX1);
			break;
		case 1:
			ssbi(PORT_AUX2);
			break;
		case 2:
			ssbi(PORT_AUX3);
			break;
		case 3:
			ssbi(PORT_AUX4);
			break;
		default:
			break;
		}
	}
}
